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Abstract 


The  problem  of  estimating  the  trajectory  of  a  maneuvering  target 
using  passive  sonar  measurements  obtained  from  an  array  of  stationary 
sensors  is  investigated.  The  formulation  is  considered  in  a  two- 
dimensional  rectangular  coordinate  system.  The  unknown  acceleration 
components  are  modelled  as  Brownian  motion  processes  and  consequently 
the  dynamic  model  is  linear.  The  types  of  measurements  used  in  the 
estimation  process  are  the  frequency  and  the  bearing  angle  of  some 
sound  signal  emanating  from  the  target.  These  measurements  are  nonlinear 
functions  of  the  state  vector  which  consists  of  a  reference  frequency 
and  the  components  of  position,  velocity  and  acceleration.  Computation 
algorithms  for  Extended  Kalman  Filter  and  "batch-sequential"  filter 
are  presented.  Equations  for  including  the  effects  of  process  noise 
on  the  batch  solution  are  derived  and  the  computation  algorithm  is 
also  given.  The  performance  of  these  filters  is  compared  using  noisy 
measurements  simulated  for  two  different  scenarios  with  typical  target 
maneuvers  and  sensor  locations.  Extended  Kalman  Filter  is  found  to 
be  the  best  in  terms  of  computation  time  and  accuracy  of  the  estimated 
trajectory.  Sensors  located  as  far  apart  as  feasible  yield  better 
results  than  those  which  are  closer  to  each  other. 
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1.  Introduction 


The  problem  of  estimating  the  trajectory  of  a  maneuvering  target 
is  one  of  the  more  difficult  of  the  current  estimation  problems.  For 
military  applications  the  target  may  be  an  airborn  venicle,  a  surface 
ship  or  an  underwater  submarine.  Though  there  are  similarities  in 
the  problem  formulation  and  in  the  methods  of  solution  for  these 
targets,  the  motion  characteristics  and  the  data  required  to  track 
the  motion  are  distinctly  different.  The  airborn  targets  are  usually 
fast  moving  and  highly  maneuverable,  while  the  underwater  targets 
are  slower  moving  and  have  limitations  on  their  ability  to  execute 
rapid  maneuvers.  The  measurements  made  on  the  aircraft  and  missiles 
are  of  high  quality,  usually  characterized  by  a  high  data  rate  active 
sensor  signal,  whereas  those  for  underwater  targets  are  obtained  pri¬ 
marily  from  passive  signals  badly  corrupted  by  the  environment  in 
which  they  travel.  In  this  report,  primary  attention  will  be  given 
to  the  problem  of  estimating  the  trajectory  of  a  single  unknown  un¬ 
derwater  target.  As  a  consequence,  attention  is  directed  to  the 
question  of  modeling  the  motion  of  the  maneuvering  target  and  to  the 
questions  of  the  structure  of  the  estimation  algorithm  used  to  process 
the  measurements. 

Statistical  estimation  techniques,  set  in  the  framework  of  modem 
control  theory,  are  applied  to  obtain  a  solution  to  this  problem.  With 
all  the  versatility  of  linear  filtering  theory,  there  are  several 
unresolved  aspects  related  to  the  tracking  of  maneuvering  targets. 

The  Extended  Kalman  Filter  (EKF)  [1],  which  is  a  local  optimal  nonlin¬ 
ear  filter,  represents  the  best  contemporary  approach.  However  this 
method  encounters  difficulties  due  to  filter  divergence;  an  effect 
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caused  by  modeling  errors  arising  from  incomplete  a  priori  knowledge 
of  the  nature  of  the  target  maneuvers.  Depending  on  the  coordinate 
system,  either  the  dynamics  or  the  observation  models  or  both  will 
be  nonlinear.  The  optimal  solution  to  nonlinear  estimation  problems 
is  extremely  difficult  to  obtain.  For  example,  finding  the  minimum 
variance  estimate  involves  the  computation  of  the  mean  of  the  con¬ 
ditional  probability  density  function  of  the  state  for  a  given  set  of 
observations,  and  this  in  turn  will  require,  in  a  general  nonlinear 
problem,  the  solution  of  a  partial  differential  equation  which  de¬ 
scribes  the  evolution  of  the  conditional  probability  density  function 
[2].  Even  assuming  that  the  noise  in  the  measurement,  the  noise 
influencing  the  system  dynamics  and  the  a  priori  state  are  all  in¬ 
dependent  Gaussian  random  variables,  the  conditional  probability 
density  function  will  evolve  as  a  non-Gaussian  process  due  to  the 
nonlinearities  in  the  system  model.  Though  some  results  are  available 
for  particular  approximations  of  a  few  nonlinear  models,  no  satis¬ 
factory  theory  has  been  developed  for  the  general  nonlinear  problem. 
The  contemporary  approach  to  the  solution  to  this  class  of  problems  is 
to  linearize  the  problem  by  referencing  the  motion  to  a  known  solu¬ 
tion.  This  known  solution  is  selected  to  be  "close"  to  the  actual 
motion  and  the  time-rate  of  change  in  the  difference  can  be  described 
by  a  set  of  linear  differential  equations.  The  estimation  algorithm 
based  on  this  approach  is  referred  to  as  the  Extended  Kalman-Bucy 
Filter  (EKF)  [2]  [3].  The  EKF  has  been  adopted  as  the  best  choice 
for  solving  the  maneuvering  target  trajectory  estimation  problem  in 
the  majority  of  the  previous  investigations  and  this  algorithm  will 
be  used  as  the  basis  for  the  investigations  reported  here.  Having 


selected  the  EKF  algorithm,  the  next  step  is  to  find  ways  of  over¬ 
coming  the  difficulties  that  may  be  encountered  when  the  target  ex¬ 
ecutes  a  maneuver. 

Several  authors  [A]  have  discussed  the  problem  formulation  for 
a  single  observer  tracking  a  maneuvering  target.  They  have  consid¬ 
ered  two  dimensional  motion  in  a  rectangular  coordinate  system.  The 
acceleration  is  assumed  to  be  zero  and  the  state  vector  consists  of 
position  and  velocity  components.  In  the  single-observer  tracking 
problem  using  only  bearing  measurements,  the  observer  must  know  his 
own  motion  and  must  execute  a  maneuver  within  the  time  period  of 
the  observation  set  if  the  target  state  to  be  observable.  The 
Extended  Kalman  Filter  is  used  in  these  investigations  with  different 
variations  in  the  implementation.  Alspach  [5]  discusses  an  approximate 
nonlinear  estimation  approach  based  on  an  optimal  Bayesian  filter 
implementation  by  the  Gaussian  sum  approach.  The  results  presented 
demonstrate  the  difference  between  this  method  and  the  EKF  approach. 
Moose  and  McCabe  [6]  consider  a  two  dimensional  problem  with  passive 
time  delay  measurement  and  Doppler  frequency  measurement.  Their  ap¬ 
proach  is  to  model  the  target  dynamics  by  assuming  a  time-correlated, 
randomly  switching  mean  forcing  function  as  a  deterministic  input 
command,  and  the  single  observer  is  assumed  to  be  moving  with  con¬ 
stant  velocity.  The  EKF  is  used  to  estimate  the  position  and  velocity 
in  a  relative  polar  coordinate  system  referenced  to  the  moving  observ¬ 
er. 

The  problem  considered  in  this  investigation  is  one  of  estimating 
the  trajectory  of  a  target  moving  intwo  dimensions,  using  measure¬ 
ments  obtained  from  an  array  of  stationary  or  drifting  sensors.  The 
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types  of  measurements  are  the  'Doppler  frequency'  of  some  sound  signal 
emanating  from  the  target  and  the  direction  from  which  this  sound 
signal  is  coming.  It  should  be  noted  that  in  an  actual  target  motion 
analysis  application  these  measurements  must  be  considered  in  three 
dimensions  due  to  the  nature  of  signal  propagation.  Though  the  formu¬ 
lation  is  no  more  complicated  (except  for  the  algebra)  than  the  two 
dimensional  application,  in  order  to  demonstrate  filtering  approaches, 
we  have  used  the  two  dimensional  model. 

Coordinate  System 

There  are  several  choices  for  the  coordinate  systems  used  in  the 
mathematical  formulation  of  the  maneuvering  target  problem,  and  a 
'natural'  coordinate  system  would  be  the  polar  coordinates  from  each 
sensor  with  each  sensor  in  turn  related  to  a  common  reference  frame. 
Another  convenient  choice  is  a  single  polar  coordinate  system  to 
which  measurements  from  all  the  sensors  are  referred.  However,  sel¬ 
ection  of  either  of  these  co-ordinate  systems  will  lead  to  a  com¬ 
plicated  nonlinear  model  for  representing  the  dynamical  motion.  Hence 
a  fixed  rectangular  coordinate  system  is  chosen  for  the  formulation. 

In  this  reference  frame,  the  dynamics  are  linear  (except  for  some 
advanced  models),  but  the  observation-state  relation  is  nonlinear. 

To  illustrate  the  effect  of  the  co-ordinate  system  selection, 
let  the  simplest  estimation  state  vector  for  the  polar  coordinate  sys¬ 
tem  shown  in  Figure  1,  be  taken  as, 

xT  =  [f  P  0  P  0]  =  [xx  x2  x3  x4  x5]  (1.1) 
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so  that 


x  =  f(x,t)  +  w 

where 


X1 = 

f1(x,t) 

=  0 

X2  = 

f9(x,t) 

=  X4 

X3  = 

f3(x,t) 

=  X5 

II 

•  X 

f^(x,t) 

=  Up  +  X2X5 

X5  = 

f5(x, t) 

=  (u0  -  2x4x5)/x2 

and 

wT  =  [wf  0  0  0  0] 


(1.2) 


(1.3) 


(1.4) 


In  the  above,  fQ  Is  the  reference  frequency  for  Doppler  frequency 
measurement,  p  is  the  range,  G  is  the  bearing  angle,  is  a 

zero-mean  Gaussian  white  noise,  and  u  and  are  the  input  accel¬ 

eration  components  in  the  radial  and  transverse  directions  respective¬ 
ly.  The  (  )  indicates  differentiation  with  respect  to  time.  For  the 
case  with  polar  coordinates  referenced  to  a  single  sensor,  the  compon¬ 
ents  of  x  refer  to  the  relative  motion  of  the  target  with  respect 
to  that  sensor.  As  shown  in  Fig.  1.1,  the  observation-state  relation 
for  the  bearing  angle  measurement  is  given  by 


Cp  =  0(x)  +  e0  =  x3  +  eq  (1.5) 

For  the  formulation  in  a  polar  coordinate  system  common  to  all  the 
sensors,  the  dynamics  refers  to  the  position  and  velocity  of  the  tar¬ 
get  with  respect  to  the  origin  of  the  coordinate  system.  To  simplify 
the  algebra,  consider  one  of  the  sensors,  located  as  shown  in  Fig.  1.2. 


Then  i he  observation-state  relation  for  the  bearing  angle  measurement 
is  obtained  from  the  relation. 


"i  ’  "  +  “i 


(1.6) 


To  compute  «  ,  note  that 


cos  oij  +  cos  ft*  =  p 


=  p^  +  -  2p  cos  O'  and  O' 


It  then  follows  that 


GB  =  pi  +  i:6 


=  x^  +  cos 


x2  •  'l  cos  x3 


A?” 


- +  , 


^  .  *-  ry  J  | 

^  +  x2  -  cos  x3 


and  =  x^  -  — 


Thus,  even  if  the  bearing  angle  measurement  is  the  only  measurement 
type,  in  the  latter  system  it  is  nonlinear.  Furthermore,  the  second 
type  of  measurement  to  be  considered,  namely,  the  Doppler  frequency 
has  nonlinear  forms,  in  both  of  the  above  two  polar  coordinates.  In 
tiie  former  system,  along  with  Equation  1.5,  the  Doppler  measurement  is 
given  by 


G  =  - H -  +  t;  = - - - 

f  •  f  x. 

(1  +  J)  (1  +  -±) 


(1.10) 
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In  the  latter  system  to  obtain  the  relation  between  the  state  and 
frequency  measurement,  note  that 


_1_ 

Pi 


+  p2  -  2L^p  cos  8* 

(pp  -  L^p  cos  8'  +  L^p0  sin  0') 


Then 


Cf  =  fl  +  £f  = 


”* —  +  £  _ 


(i  +  — ) 

c 


or 


-T —  + 

Pi  f 


(1  +  -i) 

c 


(1.11) 


(1.12) 


where 


Pi  = 


x„x.  -  L,x,  cos  x'  +  L,x„xc  sin  x' 
24  14  3  125  3 


^1  +  4  -  2L1X2  COS  *3 


(1.13) 


Hence  in  both  the  coordinate  systems  at  least  one  type  of  measurement 
is  nonlinear  and  the  dynamics  are  always  nonlinear.  In  a  rectangular 
coordinate  system,  as  will  be  shown  in  Chapter  3,  the  dynamic  model 
is  linear  and  although  both  the  measurement  types  are  nonlinear,  the 
algebraic  expressions  are  not  as  complicated  as  those  given  in  Eqs. 
(1.9)  through  (1.13).  Consequently,  in  the  investigation  described 
in  the  following  chapters,  the  rectangular  coordinate  system  is 
chosen  to  exploit  the  linearity  in  the  dynamic  model. 

For  this  investigation,  the  estimation  state  vector  will  consist 
of  the  components  of  position,  velocity  and  acceleration  and  also  the 
reference  frequency  for  the  received  signal.  The  reason  for  estimating 
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the  acceleration  components  is  as  follows.  The  acceleration  of  the 
target  cannot  be  known  for  the  entire  period  of  interest.  While  the 
target  acceleration  is  zero  for  some  periods  of  time,  there  will  be 
other  periods  during  target  maneuvers  when  it  will  have  a  significant 
acceleration.  Consequently,  it  is  important  to  model  the  acceleration 
in  such  a  way  that  the  filter  can  detect  any  maneuver  that  the  target 
might  execute  and  track  it  as  accurately  as  possible.  One  approach  to 
satisfying  this  requirement  is  to  model  the  acceleration  components 
as  Brownian  motion  processes.  This  model  has  been  used  to  estimate 
the  accelerations  due  to  unmodeled  forces  in  a  number  of  previous  in¬ 
vestigations.  See  for  example.  References  [7],  [8]  and  [9].  By 
using  this  approach  the  time  rate  of  change  of  the  acceleration  com¬ 
ponents  can  be  included  in  the  dynamic  model  as  white  noise.  This 
representation  is  adopted  for  the  model  described  in  the  following 
chapter. 

An  important  requirement  for  the  success  of  the  Extended  Kalman 
Filter  throughout  the  estimation  time  period  is  that  an  acceptable 
initial  estimate  for  the  trajectory  be  available  to  satisfy  the  as¬ 
sumptions  involved  in  linearizing  the  original  nonlinear  problem. 

One  way  of  achieving  this  estimate  is  to  process  a  limited  set  of 
measurements  with  a  batch  estimation  algorithm  to  obtain  a  solution 
for  the  initial  state  and  the  error  covariance  matrix.  This  infor¬ 
mation  can  be  used  as  the  a  priori  state  and  the  a  priori  covariance 
matrix  to  start  the  EKF.  When  the  state  error  covariance  and  con¬ 
sequently  the  gain  decays  to  some  steady  state  values  (small),  at 
a  later  state,  the  filter  may  saturate  and  may  not  be  able  to  detect 
a  possible  maneuver  of  the  target  and  consequently  may  diverge.  One 
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approach  to  avoid  this  situation  would  be  to  divide  the  period  of 
interest  into  a  number  of  smaller  arcs  and  find  the  iterated  least- 
square  solution  for  each  arc  by  propagating  one  arc  solution  (esti¬ 
mate  and  covariance)  to  the  next.  This  method  obviously  will  be  time 
consuming  and  does  not  provide  a  real-time  solution,  although  it  does 
provide  an  on-line  solution  with  some  lag.  The  primary  purpose  of 
such  an  approach  is  to  avoid  the  divergence  associated  with  the  Kalman 
(or  sequential)  filter.  An  alternate  procedure  for  using  the  batch 
estimation  algorithm  is  as  a  initializer  for  the  EKF  or  sequential 
processing  algorithm.  Both  approaches  are  considered  in  the  subse¬ 
quent  investigation. 

Finally,  during  this  investigation,  a  sequential  batch  algorithm 
is  developed  by  including  the  effects  of  process  noise  in  the  a  priori 
batch  estimation  algorithm  [3].  The  development  of  this  algorithm 
is  described  and  in  the  numerical  studies  it  is  compared  with  the 
EKF  algorithm  to  determine  the  relative  accuracy  of  the  methods  in 
estimating  the  motion  of  a  maneuvering  target. 


. _ Theoretical  Background 


The  linear  state  estimation  problem  is  concerned  with  finding 
the  best  estimate  of  the  state,  x  £  Rn,  of  a  linear  dynamical  sys¬ 
tem  whose  time-evolution  is  characterized  by  the  set  of  differential 
equations , 

x(t)  =  A(t)x(t)  +  B(t)w(t)  (2.1) 

from  a  set  of  observations  Y  e  Rm,  made  on  the  system  output  x, 
where  the  linearized  observations,  y  £  Rm  ,  are  related  to  the  state 
at  discrete  epochs,  t^,  t^ ,  . t^,  through  the  following  linear  re¬ 
lation, 

=  Hf  xi  +  Ef,  i  =  1,  2,  ...,  Z  (2.2) 

The  subscript,  i,  represents  the  discrete  observation  epochs.  In 
the  above  equations,  A(t)  and  B(t)  are  called  the  system  matrices, 
w(t)  £  R^  is  a  zero  mean  Gaussian  random  disturbance  vector  and  £^ 
is  the  noise  in  the  i*"'1  observation.  Note  that  each  y^  could  be 
a  vector,  say,  y_^£  R^,  in  which  case  m  =  £  *  p.  For  the  general 
non  linear  estimation  problem,  one  would  have  a  dynamical  system  rep¬ 
resented  by 

X(t)  =  F(X(t),w(t),t)  (2.3) 

and  measurements  given  by 

Y  =  G(X(t1),  t±)  +  £±,  i  =  1,  2,  ....  X,  (2.4) 

where  F(*)  and  G(«)  are  nonlinear  functions  of  the  state  X(t) . 
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Then  Eqs.  (2.1)  and  (2.2)  would  correspond  to  a  linearized  system 

obtained  by  expanding  Eqs.  (2.3)  and  (2. A)  about  some  reference  solu- 
*  ~ 

tion,  X  (t).  The  matrices  A(t)  and  would  be  obtained  from 

F(*)  and  G(*)  by  the  following  definitions: 


A(t)  = 


3F 

ax 


*  ,  u  SG 

x  =  x*(t)  and  Hi  =  ax 


X.  =  X  (t.) 
1  1 


Accordingly,  x(t)  in  Eq.  (2.1)  represents  the  deviation  from  a 

* 

reference  trajectory  X  (t)  satisfying  Eq.  (2.3),  where  w(t)  =  0 
* 

and  X(t  )  =  X  ,  a  known  vector,  and  y^  in  Eq.  (2.2)  is  the  lin- 

* 

earized  observation  deviation  from  the  reference  value  G(X  (t.),  t.). 

i  l 

The  term  B(t)w(t)  in  Eq.  (2.1)  is  the  process  noise  term  added  to 

compensate  for  the  incomplete  modeling. 

I 

Given  the  observation  set  Y  =  (Y^,  . . . ,  }  ,  the  estimate 

of  the  state  may  be  desired  either  at  some  particular  epoch,  say  tQ, 

which  we  will  call  the  batch  solution  x  =  x(t  /t„)  or  estimates 

o  o  Jc 

of  the  state  may  be  desired  at  each  time  point  t^,  (t^  £  t^  £  t^) , 
based  on  all  the  observations  up  to  that  time,  Y1  =  { Y^ ,  ...,  Y^}  . 
This  latter  solution  is  referred  to  as  the  filtered  solution 
x^  =  x(t^/t^).  To  compute  the  batch  estimate,  Eqs.  (2.2)  are  ex¬ 
pressed  in  terms  of  the  state  at  the  epoch  tQ,  as 

y.  =  H.  x  +  n.  ,  1*1,  ...,  £  (2.5) 

i  l  o  i 


where 


Hi  “  Hi  *(ti’to) 


ni  =  **ij  <5>(ti>T)B(r)w(T)dT  + 

t 

o 


(2.6) 


*  4 
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The  nxn  matrix  $(t^,t.)  is  referred  to  as  the  state  transition 
matrix  for  the  matrix  A(t)  [2]  [3].  The  batch  solution  for  a  linear 
dynamical  system  with  the  process  noise  w(t) ,  has  exactly  the  same 
structure  as  the  solution  to  the  system  with  w(t)  =  0  (see  Appendix 
A).  Hence,  for  the  purpose  of  reviewing  the  theory,  we  consider 
only  the  homogeneous  part  of  Eq.  (2.1),  namely  the  system: 

x ( t )  =  A(t)  x ( t )  .  (2.7) 

Then,  in  Eq.  (2.6)  and  Eq.  (2.5)  becomes 

y±  ~  ^  1  —  1*  •••»  S-  (2.3) 

This  sequence  of  observations  can  be  collected  into  a  single  matrix 
as  follows : 

y  =  H  xq  +  e  (2.9) 

where 


V 

V 

ei 

• 

• 

• 

y  = 

• 

• 

•  • 

II 

PS 

,  and  £  = 

• 

• 

• 

!  • 

LV 

% 

-V 

Now  the  problem  reduces  to  solving  the  linear  system  in  Eq.  (2.9) 

for  x  . 
o 

Because  of  the  noise  present  in  the  observations,  it  is  not 
possible  to  compute  xq  from  y  deterministicallly  and  some  criter¬ 
ion  must  be  used  to  obtain  a  solution.  Using  the  principle  of  least 
squares,  [2]  [3]  the  optimal  estimate  of  xq  is  selected  to  minimize 


the  sum  of  the  squares  of  the  observation  residuals,  calculated  from 
an  assigned  value  of  xq.  In  this  approach,  the  quantity  is 

interpreted  not  as  a  random  noise  but  as  an  error  arising  from  lin¬ 
earization  mismodeling  and/or  the  specification  of  an  incorrect  value 
for  x^.  The  solution  is  obtained  by  solving  a  deterministic  opti¬ 
mization  problem  for  which  the  performance  index  is  some  function  of 
the  errors  e^.  The  commonly  defined  performance  index  is  the  weighted 
sum  of  the  errors  expressed  as 

J  =  eTR-1t  (2.10) 

where  R  is  the  inverse  of  the  weights  assigned  to  the  observation. 

For  the  minimum  variance  or  maximum  likelihood  estimates,  R  is  the 
covariance  matrix  associated  with  the  set  of  observation  errors,  £. 

The  result  of  minimizing  J,  as  defined  in  Eq.  (2.10),  leads  to  the 
following  estimate  [3], 

xq  =  (HTR_1H)_1  (HTR_1y)  (2.11) 

In  order  to  avoid  possible  numerical  difficulties  and  to  make  use 
of  previous  knowledge,  a  priori  information  can  be  included  in  the 
solution.  This  is  equivalent  to  considering  a  performance  index 

J  =  t;TR_1e  +  (x  -x  )T  P  _1  (x  -x  )  (2.12) 

o  o  o  o  o 

where  x  and  P  are  the  a  priori  state  and  the  state  error  covar- 
o  o 

iance  respectively.  These  statistics  represent  all  information  on 
the  estimate  of  the  state  prior  to  obtaining  the  observation  set,  y. 
The  solution  obtained  by  using  the  a  priori  information  is  given  by 
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x  =  (HTR~LH  +  P  L)  1(HIR~1y  +  P  1x  )  (2.13) 

o  o  o  o 

This  estimate  of  the  state  deviation,  Xq ,  is  added  to  the  a  priori 
* 

state  Xq,  to  obtain  the  least  squares  batch  estimate  for  the  initial 
state  of  the  nc nlinear  system  at  epoch  tQ,  i.e., 

X  =  X*  +  x  (2.14) 

o  o  o 

Eq.  (2.13)  could  be  the  solution  to  a  parameter  estimation  prob¬ 
lem,  or  to  the  problem  of  state  estimation  of  a  linear  dynamical  sys¬ 
tem  at  some  sptcified  time  epoch.  More  often  the  best  estimate  of 
the  state  of  a  dynamical  system  may  be  desired  at  different  time 
points  rather  than  at  a  single  epoch.  Though  this  could  be  obtained 
with  Eq.  (2.13)  at  desired  time  points,  a  powerful  recursive  algor¬ 
ithm  designed  by  Kalman  [1],  is  the  most  widely  used  technique  due  to 
its  efficiency  in  processing  the  measurements  sequentially.  This 
algorithm  can  be  derived  in  several  different  ways  based  on  different 
principles.  In  the  most  direct  approach,  it  can  be  derived  from 
Eq.  (2.13)  using  an  identity  in  matrix  algebra  known  as  the  Schurr 
identity,  [10]  which  can  be  stated  as  follows: 

(BA~1BT  +  C-1)"1  =  C  -  CB(BTCB  +  A)-Vc  (2.13) 

Suppose  that  the  estimate  x(k/k)  of  the  state  at  a  time 
t  (t  £  t,  <  t  )  based  on  observations  up  to  t,  ,  y  ,  is  to  be  com- 
puted  recursively  from  the  estimate  x(k-l/k-l)  and  its  covariance 
P(k-l/k-l)  at  time  tk_1 .  x(0/0)  and  P(0/0)  are  assumed  to  be 

given.  Then  the  estimate  and  covariance  at  t^_^  are  Pr0Pagated 
according  to  Eq.  (2.7)  to  obtain  the  a  priori  estimate  3(k/k-l)  and 
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its  covariance  P(k/k-l)  as 


*k 


=  x(k/k-l)  =  4>(k,k-l)x(k-l/k-l) 


(2.16) 


P  =  P(k/k-l)  =  4>(k,k-l)P(k-l/k-l)$  (k,k-l) 


(2.17) 


Now,  Eq.  (2.15)  can  be  applied  to  Eq.  (2.13)  if  the  matrices  H,  R, 

P  ,  y  and  xq  are  replaced  by  the  appropriate  quantities;  in  other 
words,  Eq.  (2.13)  is  rewritten  as 


S(k/k>  -  (Hk\'\  +  + 


(2.13a) 


which  implies  that  the  new  measurement  y^  with  an  error  whose 

covariance  is  R^,  is  combined  with  the  a  priori  estimate,  x^,  by 
minimizing  an  appropriate  performance  index  similar  to  the  one  de¬ 
fined  in  Eq.  (2.12).  Application  of  Eq .  (2.15)  to  (Eq.  2.13a)  leads 
after  some  algebraic  manipulation,  to  the  following  expression.  [3] 


x(k/k)  =  Xj^  +  \(yk  “  (2.18) 

where 

Kk  ■  tt’vA’ -  V1  u-19) 

The  error  covariance  of  this  estimate  is  given  by 

P(k/k)  =  Pfc  -  KkHkPk  (2.20) 

Equations  (2.16)  through  (2.20)  define  the  equations  needed  for  the 
recursive  filtering  algorithm  derived  by  Kalman  [1].  The  estimate 
X(k/k)  of  the  state  for  the  nonlinear  system  is  computed  by  adding 
the  estimate  of  the  deviation  x(k/k)  from  Eq.  (2.18)  to  the 


18 


X  • 

nominal  value  X  (k)  obtained  by  integrating  X  in  Eq .  (2.3)  from 

*  * 

to  with  X  (k-1) .  X  (0)  is  assumed  to  be  given. 

The  Extended  Kalman  Filter  is  the  same  as  the  above  algorithm 
except  for  the  reference  trajectory  and  Eq.  (2.18).  The  reference 
trajectory  is  updated,  after  processing  each  observation,  with  the 
estimate  of  the  deviation.  In  other  words  X  in  Eq .  (2.3)  is  inte¬ 
grated  from  tfcl  to  tk  with  X(k-l/k-l)  instead  of  X*(k-1), 
to  obtain  X(k)  which  is  updated  by  the  estimate  of  the  deviation 

x(k/k)  =  (2.21) 

and  the  estimate  at  t^,  then  is 

X(k/k)  =  X(k)  +  x(k/k)  (2.22) 

It  should  be  noted  that  the  measurement  partials  are  to  be  eval¬ 

uated  using  the  state,  X(k) .  Thus  the  EKF  algorithm  is  given  by 
Eqs .  (2.16),  (2.17)  and  (2.19)  through  (2.22). 

The  least-squares  solution  given  in  Eq.  (2.13)  involves  the  in¬ 
version  of  an  n  x  n  matrix,  n  being  the  dimension  of  the  state 
vector,  (usually  R  is  assumed  to  be  a  diagonal  matrix  and,  hence, 

involves  only  scalar  inversion)  and  on  many  occasions  due  to  the 

T  -1  — 1 

ill-conditioned  nature  of  the  n  x  n  matrix,  (HR  H  +  ) ,  its  in¬ 

version  will  result  in  an  inaccurate  solution.  This  numerical  diffi¬ 
culty  can  be  circumvented  if  the  least-squares  problem  is  formulated 
in  a  different  way.  The  solution  represented  by  Eq.  (2.13)  is  based 
on  the  'normal  equations'  formed  by  minimizing  the  performance  index 
in  Eq.  (2.12).  An  alternate  least-squares  solution  to  the  problem 
can  be  obtained  if  one  uses  the  'data  equations'  involving  the 
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T  -1  — 1 

information  matrix  (HR  H  +  P  ).  This  solution,  discussed  in  Ref. 

o 

[11]  and  [12]  is  based  on  the  use  of  a  sequence  of  orthogonal  trans¬ 
formation  matrices  to  triangularize  the  information  matrix.  The 
solution  to  the  estimation  problem  is  obtained  by  a  sequence  of  back 
substitutions.  For  the  scenarios  considered  in  this  investigation, 
we  did  not  encounter  any  serious  problem  in  matrix  inversion,  partly 
due  to  the  word  length  of  the  Cyber  170/750  computer  used  for  the 
study.  Consequently,  we  do  not  present  numerical  results  obtained 
with  this  approach.  However,  in  an  actual  target  tracking  applica¬ 
tion  where  a  computer  with  a  smaller  word  length  must  be  used,  the 
matrix  triangularization  approach  can  lead  to  a  more  accurate  solu¬ 
tion  than  the  normal  matrix  approach.  Hence,  a  brief  account  of  the 
approach  is  given  in  the  following  discussion  and  a  useful  computa¬ 
tional  algorithm  is  described  in  Chapter  5.  The  method  could  be  ob¬ 
tained  by  requiring  that  the  performance  index,  J,  be  minimized,  where 


Then  if  T  is  an  m  *  m  orthogonal  matrix  [11],  it  follows  that 


J  =  (y-HxQ)TTTT  (y-HxQ) ,  T  orthogonal, 


TY-THx 


If  furthermore,  T  is  selected  so  that  TH 

A 

matrix  R,  is  upper  triangular,  it  follows  that 


where  ,  the  n  x  n 


J  =m[5j  xc 


ir 


(£-Rxq)  j  |2  +  | I e || 2 


(2.23) 
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J  is  minimum  when  z  -  Rx  =0  and  thus  the  solution  is  obtained 

o 

from  the  relation. 


(2.24) 


The  sum  of  the  observation  residuals  is  given  by  the  remainder  in 

Eq.  (2.23),  i.e.,  J  =  £r^  =  j|e||^.  Since  orthogonal  matrix  T  is 

chosen  such  that  the  matrix  R  is  an  n *  n  upper  triangular  matrix 

Eq.  (2.24)  can  be  used  to  obtain  xq  by  back  substitution.  When 

there  is  a  weighting  matrix  R  ^  in  the  performance  index  as  in  Eq. 

(2.10),  the  observation  vector  y  and  the  matrix  H  are  normalized 

-1/2 

by  multiplying  each  by  R  which  amounts  to  scaling  the  H  and 

y  matrices  by  the  standard  deviations  of  the  observations.  This 
approach  can  be  extended  in  a  straight  forward  manner  to  include  the 
information  contained  in  the  a  priori  estimate  and  covariance  matrix. 
The  a  priori  estimate  xq  and  Pq  is  converted  into  a  data  equation 
by  writing  Pq  in  terms  of  its  square  root  matrix  U,  such  that 

-  T 

Pq  =  UU  .  Using  the  Cholesky  decomposition  method  [11],  U  can  be 
determined  as  an  upper  triangular  matrix,  and  if  R  =  U  \  then 
the  data  equation  corresponding  to  the  a  priori  information  is 


z  =  Rx  +  £ 
o 


(2.25) 


where  z  =  Rxq  .  This  is  combined  with  the  observation  Eq.  (2.9), 
after  normalization,  as  follows. 

T 

J  =  e  e  +  e  e 

=  (y  -  Hx  )T(y  -  Hx  )  +  (z  -  Rx  )T(z  -  Rx  ) 

7  o  o  o  o 

=  I  lY  -  Hxcl  I2  +  1 1 2  -  Rxj  I2 
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which  is  again  the  same  as  Eq.  (2.23)  and,  hence,  the  solution  is  still 
of  the  form  given  in  Eq.  (2.24).  There  are  two  types  of  orthogonal 
matrices  which  will  produce  the  same  result  of  matrix  triangulariza- 
tion,  namely.  Householder  transformation  [11]  [12]  and  the  Givens 
rotation  [11].  It  is  important  to  note  that  the  above  method  of  tri- 
angularization  by  orthogonal  matrices  will  hold,  only  if  the  informa¬ 
tion  array  H  and  the  observations  y  are  normalized  so  that  the 
error  e  has  zero  mean  and  unit  variance;  however,  the  method  can  be 
readily  extended  to  the  case  where  the  observation  covariance  matrix 
is  not  the  identity  matrix,  I. 

As  is  obvious,  the  solution  by  use  of  orthogonal  transformation 
matrices,  has  been  described  above  for  the  least-square  problem.  The 
method  can  be  extended  to  the  sequential  estimation  process  also. 
Besides  numerical  accuracy,  another  attractive  aspect  of  the  ortho¬ 
gonal  rotation  method  is  that  the  orthogonal  matrix  T,  required  to 
convert  the  m  x  n  matrix  H  into  the  n  x  n  upper  triangular  matrix, 

A 

R,  does  not  have  to  be  computed  and  stored  explicitly.  The  trans¬ 
formation  is  implicit  and  it  is  performed  recursively  on  the  columns 
of  the  information  array.  For  a  detailed  discussion  of  the  numerical 


characteristics  of  this  method,  one  may  refer  to  Lawson  and  Hansen 
[11]  and  Bierman  12], 


3.  Dynamical  Model  for  a  Maneuvering  Target 

The  best  strategy  for  the  solution  of  the  problem  of  estimating 
the  trajectory  of  a  moving  target,  using  passive  sonar  frequency  meas¬ 
urements  and  bearing  angle  measurements  obtained  by  a  set  of  observers, 
is  discussed  in  this  chapter.  The  target  under  consideration  is  as¬ 
sumed  to  be  a  slow  moving  object  incapable  of  making  complicated 
maneuvers  within  a  short  period  of  time.  However,  it  can  accelerate 
(decelerate)  and  can  execute  certain  patterns  of  maneuvers.  The 
maneuvers  are  of  unknown  magnitude  and  direction  and  occur  at  unknown 
times.  At  a  given  instant,  the  target  could  be  in  any  one  of  the  fol¬ 
lowing  states  -  it  could  be  at  rest  or  it  could  be  moving  with  constant 
velocity  or  it  could  be  executing  a  maneuver.  Any  proposed  dynamic 
model  must  be  able  to  accomodate  all  the  states  of  motion  and  the 
state  of  rest.  It  is  obviously  impossible  to  assume  an  expression 
for  the  acceleration  which  will  describe  all  possible  maneuvers.  How¬ 
ever,  a  mathematical  model  has  to  be  assumed  which  is  simple  enough 
to  allow  efficient  computations  and  which  can  be  used  to  estimate  the 
general  characteristics  of  an  accelerating  target.  A  simple  model 
which  satisfies  these  requirements  is  described  below. 

The  problem  of  modeling  unknown  accelerations  in  a  different 
practical  situation  has  been  considered  by  Tapley  and  Ingram  [7], 

Tapley  and  Schutz  [8]  and  Tapley  and  Hagar  [9]  among  others.  Since 
a  deterministic  expression  cannot  be  found  for  the  unknown  accelera¬ 
tion,  it  is  assumed  to  be  a  random  process  with  known  statistics.  The 
acceleration  components  in  a  rectangular  coordinate  system,  for  exam¬ 
ple,  can  be  modeled  as  (1)  Brownian  motion  processes  (zeroth  order 
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Gauss  Markov  processes)  or  as  (2)  first  order  adaptive  Gauss  Markov 
processes  driven  by  white  noise  or  as  (3)  second  order  adaptive  Gauss 
Markov  process  driven  by  white  noise.  These  models  are  of  increasing 
complexity  in  the  order  mentioned  above.  The  first  and  second  order 
adaptive  Gauss  Markov  process  models  are  nonlinear  dynamic  models 
whereas  the  Brownian  motion  model  is  a  linear  dynamic  model.  Further, 
the  dimension  of  the  state  vector  and  hence  the  computation  time  in¬ 
creases  with  the  order  of  the  acceleration  model.  Large  dimension  of 
the  state  vector  could  cause  numerical  difficulties  in  matrix  inver¬ 
sion.  Based  on  these  considerations  we  model  the  acceleration  compo¬ 
nents  as  Brownian  motion. 

The  target  motion  is  assumed  to  occur  in  two  dimensions  only  and 
the  estimation  state  vector  will  contain  the  position,  velocity  and 
acceleration  components.  Use  of  sensor  frequency  measurements  in  the 
estimation  process,  involves  assumption  of  a  reference  frequency  fQ. 
Since  the  reference  frequency  may  not  be  known  exactly,  it  is  necess¬ 
ary  to  include  f  in  the  estimation  state  vector.  Thus,  for  the 
o 

model  considered  in  this  investigation  the  state  vector  will  have 
seven  components  -  two  each  for  position,  velocity  and  acceleration 
of  the  target  and  one  for  the  reference  frequency.  Under  these 
assumptions,  the  equations  of  motion  and  their  solution  are  given  in 
the  following. 

The  time  rate  of  change  of  acceleration  components  are  modeled 
as  zero  mean  Gaussian  white  noise,  namely, 

a  =  w  (3.1) 

x  1 


a  =  w„ 

y  2 


(3.2) 
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where  w^  ~  N(0,q^),  i  =  1,  2.  That  is,  each  acceleration  component 
is  assumed  to  be  a  zero  mean  Gaussian  process  with  variance,  q^. 

Then  the  solution  (in  the  mean)  to  the  equations  of  motion  will  be 
given  by  [2] 


x(t)  =  x(0)  +  v  (0) t  +  ^  a  (0)t2 

X  L  X 


y(t)  =  y (0)  +  Vy(0)t  +  J  ay(0)t2 


v  (t)  =  v  (0)  +  a  (0)t 

XXX 


v  (t)  =  v  (0)  +  a  (0)t 

y  y  y 


a  (t)  =  a  (0) 
x  x 


(3.3) 
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The  reference  frequency  f  is  basically  a  constant  value.  Since 
the  value  is  not  known  exactly,  its  time  rate  of  change  is  also  assumed 


to  be  a  white  noise  and  is  modeled  as, 


f  =  w,.  with  wc  ~  N(0,q,) 
of  ft 


(3.5) 


Then  the  equations  of  motion  of  the  target  can  be  written  as  a  set  of 


first-order  differential  equations 


f  =  w 
o  f 


y  =  v 


v  =  a 
x  x 


v  =  a 

y  y 


a  =  w 
x  1 


a  =  w„ 

y  2 


which  can  be  written  in  matrix  form  as 


X  =  AX  +  w 


(3.6) 


where 


0  0  0  0  0  0  0 


0  0  0  1  0  0  0 


0  0  0  0  1  0  0 


0  0  0  0  0  1  0 


(3.7) 


0  0  0  0  0  0  1  and  w  = 


0  0  0  0  0  0  0 


0  0  0  0  0  0  0 


Note  that  the  first-order  matrix  differential  equation  of  motion  for 
the  target  is  linear  and  the  state  equation  linearization  associated 
with  many  non-linear  estimation  applications  is  not  necessary.  In 
particular,  Eq.  3.6  does  not  have  to  be  linearized  to  use  the  esti¬ 
mation  algorithms  described  in  Ch.  2.  In  the  present  problem,  as 
modeled  above,  the  nominal  trajectory  can  be  computed  in  a  straight 
forward  manner  using  the  state-transition  matrix  for  the  matrix  A 
given  above,  which  is 


♦(t.^ 


1 

0 

0 

0 

0 

0 

0 


0 

1 

0 

0 

0 

0 

0 


0 

0 

1 

0 

0 

0 

0 


0  0  0  0 

(t-t  )  0  kt-t  )2  0 
O  Z  o 

o  (t-t  >  o  kt-t)2 

o  i  o 

1  0  (t-t  )  0 

o 

010  (t-t  ) 

o 

0  0  10 

0  0  0  1 


(3.8) 


Thus  considering  the  state  X  as  a  random  vector  governed  by  the 
differential  equation  (3.6),  its  mean  is  propagated  as 

X(t)  =  4>(t,t  )X(t  )  (3.9) 

o  o 

and  its  coveriance  is  propagated  according  to  [2]  [3] 

P(t)  =  $(t,to)Po<I>T(t,to)  +  |  <Kt,T)Q$T(t,r)dT  (3.10) 

t 

o 

where  X(t  )  and  P  are  the  a  priori  state  and  covariance  at  time 
o  o 

tQ  and  Q  is  a  7  *  7  matrix  with  zeros  everywhere  except  q^,  q£  and 


q^  on  the  second,  third  and  seventh  diagonal  elements  respectively. 


4.  Measurement  Model 


Introduction 

Measurements  of  two  basic  types  are  available  for  use  in  esti¬ 
mating  the  motion  of  a  meaneuvering  target.  These  measurements  are 
1)  the  frequency  of  the  received  signal  and  2)  the  bearing  angle 
between  the  signal  source  and  some  arbitrary  reference  direction.  The 
changes  in  the  bearing  angle  and  frequency  are  assumed  to  be  related 
to  the  motion  of  the  target.  In  the  treatment  discussed  here,  the 
effects  of  multipath,  medium  inhomogenities  and  measurement  bias'  are 
neglected. 

Frequency  Measurement 

The  mathematical  model  for  the  frequency  measurement  processed 
in  the  estimation  algorithm,  is  based  on  the  principle  of  the  Doppler 
effect  [13].  When  both  the  observer  and  the  sound  source  are  station¬ 
ary,  the  observer  receives  the  true  frequency,  f^,  transmitted  by  the 
source.  If  the  observer  or  the  source  or  both  are  in  motion,  then 
the  observer  receives  either  a  greater  or  smaller  number  of  cycles 
per  unit  of  time,  depending  on  the  relative  motion  between  the  obser¬ 
ver  and  the  source.  This  difference  in  frequency  between  the  received 
and  the  transmitted  wave,  is  known  as  the  Doppler  effect  [13].  A 
mathematical  relation  exists  between  this  frequency  shift  and  the 
relative  speed  of  the  observer  with  respect  to  the  source.  Thus  by 
measuring  the  frequency  of  a  signal  and  knowing  its  actual  frequency 
an  observer  can  calculate  the  relative  speed  of  the  source.  Depend¬ 
ing  on  whether  the  source  is  moving  and  the  observer  is  stationary  or 
vice-versa,  there  are  two  different  expressions  for  this  frequency 


shift : 
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For  the  moving  source/stationary  observer  case, 


f 


c 


For  the  moving  observer /stationary  source  case 

f  =  f  (1  -  -)  (A. 2) 

o  c 

In  each  of  these  expressions,  f  is  the  measured  frequency,  f  is 

o 

the  reference  (or  actual)  frequency,  c  is  the  speed  of  sound  in 
the  wave  propagation  medium,  and  p  is  the  time  rate  of  change  of 
the  linear  distance  between  the  source  and  the  observer.  In  the 
present  investigation,  the  source  will  be  assumed  to  be  in  motion  with 
respect  to  the  observer  and  Eq.  (4.1)  is  applicable. 

The  observed  frequency  f  is  related  to  the  state  vector  through 
the  quantities  f  and  p.  This  relation  is  non-linear  and  hence 
must  be  linearized  in  order  to  be  used  in  the  estimation  algorithm. 

The  linearization  of  the  non  linear  relation  between  the  state  X 
and  the  measurement  f,  denoted  by  the  general  relation,  Y  =  G(X,t), 
involves  the  computation  of  the  first  partials  of  G  with  respect 
to  (the  components  of)  X.  For  a  single  observation,  these  partials 
form  a  (1*  n)  row  matrix,  which  will  be  denoted  by  H.  For  the  fre¬ 
quency  measurement,  the  partials  are  as  follows. 

Let  (x  ,  y  )  be  the  coordinates  of  an  observer  (which  for  this 
s  s 

•  • 

investigation  will  be  a  sonar  buoy),  (x,  y) ,  (x,  y)  be  the  compon¬ 
ents  of  position  and  velocity  of  the  target  at  a  given  instant  and 
let  f  be  the  frequency  received  at  that  instant.  Then  the  range 
and  range  rate  are  given  by 
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p  =  [(x  -  xg)2  +  (y  -  yg)2) 


1/2 


(x  -  xg)x  +  (y  -  yg)y 

p  = - - - 


With  G  =  fQ/(l  +  p/c),  we  have 


3G 

3f 


1  + 


The  remaining  partials  can  be  obtained  as  follows: 


3G  =  3G  3£ 
3x  3f  3x 


(4.3a) 


o  1  3p 

•  2  c  3x 

(1  +  7) 

c 


where 


x  -  x 


|£  =  ^  (px  -  [(X  -  xs)x  +  (y  -  ys)y]  — 
P 


px  -  p (x  -  xg) 


Using  this  procedure,  the  remaining  elements  of  H  are  determined  as 
follows 


3G 

3x 

3G 

3y 

3G 

3x 


• 

nfpx  - 

P(x  -  xg) ] 

(4.3b) 

n[py  - 

p(y  -  yg)3 

(4.3c) 

np[x  - 

Xs] 

(4.3d) 

‘.•O',* 
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3G  r  , 

SJ  ■  "Plv  -  ^s1 

(4 . 3e) 

|5--  o 

da 

X 

(4. 3f) 

y 

(4.3g) 

where 

-f 

n  =  - ^ 

cp2(l  +  £) 
c 

(4. 3h) 

Collecting  the  partials  from  Eqs.  (4.3a)  to  (4.3h),  the 

H  matrix 

corresponding  to  the  frequency  measurement  is 

~  f 3G  3G  3G  3G  3G  n  ' 

f  [_3f  3x  3y  3x  3y 

(4.4/ 

where  the  partial  derivatives  are  defined  by  Eqs.  (4.3).  The  partial 
derivatives  are  evaluated  on  some  reference  solution  assumed  to  be 
near  to  the  actual  target  motion. 

Bearing  Angle 

The  bearing  angle  of  the  target  with  respect  to  a  sensor  (ob¬ 
server)  is  another  type  of  measurement  used  in  the  estimation  algor¬ 
ithm.  It  is  defined  as  the  angle  between  the  x-axis  and  the  line 
joining  the  sensor  and  the  target  measured  in  a  direction  from  the 
x-axis  towards  the  y-axis,  as  shown  in  Fig.  1.2.  It  is  given  by 


This  measurement  is  also  a  non  linear  function  of  the  state  and  hence 
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It  must  be  linearized.  The  first  partials  of  3  with  respect  to  the 
components  of  the  state  vector  are  as  follows: 


y  -  y„ 


96  86  _  36  _  96 

3x  3y  3a  3a 


(A. 6a) 


(A. 6b) 


(A . 6c) 


(A . 6d) 


Thus  the  H  matrix  corresponding  to  the  bearing  angle  measurement 
is  given  by 


-<y  -  ys>  !  (x  "  xs)  !  I  !  I 

- 2 — ~  i  - '  °'  0  1  0  1  0 

P  !  P  1  ,  ' 


Effects  of  Measurement  Noise 

The  noise  in  the  measurement  model  is  assumed  to  be  additive; 
in  other  words,  the  measurement  Y.  obtained  at  time  t.  is  assumed 
to  be  the  sum  of  the  true  value  and  the  noise  due  to  instrument  and 
to  signal  propagation.  Since  the  true  value  is  not  known,  it  is 
assumed  to  be  close  to  the  value  computed  using  the  reference  traject¬ 
ory.  The  functional  relation  is  given  by 


h  '  G<xi''i>  +  h 


where 


G(Xi,t1)  =  f 


(1  +  £) 

c 


32 


for  the  frequency  measurement  from  Eq.  (4.1)  and 

G(X.,t  )  =  6  =  sin 

for  the  bearing  angle  measurement  from  Eq.  (4.5).  is  assumed  to 

be  a  zero  mean  Gaussian  random  variable.  Though  in  reality,  £.  is 
indirectly  dependent  on  the  range  p,  here  it  is  assumed  to  be  inde¬ 
pendent  of  the  state  vector  and  hence  it  remains  unaltered  as  an  addi¬ 
tive  term  in  the  linearized  equation  also,  as  in  Eq.  (2.2).  For 
real  observations,  the  variance  of  the  measurement  error  is  a  function 
of  the  signal  to  noise  ratio  which  depends  on  the  relative  range  and 
other  parameters.  For  the  present  investigation  we  have  assumed 
constant  values  for  the  measurement  noise  variance. 


Introduction 


For  a  target  estimation  problem  of  the  type  considered  in  this 
report,  an  on-line  sequential  filter  is  a  natural  choice.  The  Ex¬ 
tended  Kalman-Bucy  Filter  (EKF)  is  chosen  to  reduce  the  effect  of 
non  linearities  on  the  accuracy  of  the  estimate.  The  EKF  may  not 
perform  well  following  a  target  maneuver  or  when  the  filter  gains 
become  very  small.  Further,  it  may  not  always  be  possible  to  guess 
a  reasonable  a  priori  value  for  the  initial  state.  One  way  to  over¬ 
come  such  difficulties,  would  be  to  use  a  batch  algorithm  to  process 
an  initial  set  of  observations.  The  estimate  obtained  by  the  batch 
processor  can  be  used,  then,  to  start  (or  restart)  the  extended  se¬ 
quential  filter  from  the  time  epoch.  Such  a  hybrid  combination  of 
batch  and  sequential  filter  holds  promise  as  the  best  strategy  for 
estimating  the  motion  of  a  maneuvering  target  with  the  simple  model 
discussed  in  Ch.  3  and  the  computational  algorithms  for  both  of  these 
filters  are  presented  in  the  following  sections.  The  numerical  ill- 
conditioning  problem  often  encountered  in  a  batch  solution  is  elimi¬ 
nated  by  use  of  orthogonal  transformation  applied  to  the  information 
matrix,  and  t^e  steps  involved  in  computing  the  batch  solution  by 
this  approach  are  also  given.  Finally,  the  conventional  batch  esti¬ 
mation  algorithm  is  modified  to  include  the  effects  of  process  noise. 

The  Least-Squares  Algorithm 

For  the  computation  of  the  least-squares  solution  given  in 
Eq.  (2.11)  or  Eq.  (2.13),  the  matrices  HTR  and  HTR  ^y  must  be 
formed  using  all  the  measurements  available  in  a  given  batch.  In 
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the  case  of  the  conventional  least-squares  solution  for  a  system  with¬ 
out  the  random  disturbance  w  in  the  dynamics,  these  matrices  can 
be  formed  by  simple  matrix  additions  corresponding  to  each  observation. 
After  processing  all  the  observations,  either  Eq.  (2.11)  or  Eq.  (2.13), 
depending  on  whether  a  priori  information  is  to  be  included  or  not, 
is  used  to  calculate  the  batch  solution  for  a  given  time  epoch.  The 
steps  involved  in  this  computation  are  as  follows. 


Algorithm  A  —  Let  X^  be  the  given  initial  state  vector,  and  m  be 

the  number  of  measurement  to  be  processed.  The  a  priori  error  x  in 

the  initial  state  and  the  a  priori  error  covariance  P  are  option- 

o 

ally  assumed  to  be  given.  The  following  steps  lead  to  the  estimate 
of  the  state. 

0.  Initialize  the  matrices,  M,  (n  x  n) ,  where  n  is  the  dimension 

of  the  state  vector  and  L,  (n  *  1) .  Set  M  =  0,  L  =  0  and  k  =  1. 

1.  Read  the  observation:  t,  Y,  o,  the  time,  the  measurement  and  the 
measurement  standard  deviation.  Let 


tk  =  t  ;  Yk  =  Y  ;  ak  =  O  and  At  =  tk  -  t^ 

2.  Compute  the  state-transition  matrix  $(tk,  tQ) . 

3.  Integrate  X  =  AX  to  get  X(tk). 
i.e.,  compute  X(tk)  =  4>(tk,  tQ)X 

4.  Compute  the  measurement  partials 


_  9G(X,t) 
\  9X 


X  =  X(tk) 


and  Hk  =  V>(tk,  to) 


5.  Compute  the  observation  Y  and  the  observation  deviation,  y. 

Ck  K 


=  Y, 


I 


where 


2 

6.  Compute  the  weight:  R^  = 

7.  Update  the  matrices  M  and  L: 
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M '  ” +  H&\ 

L  ■ L  - 

8.  Increment  k:  k  =  k+1 

If  k  _<  m  go  to  step  1 

9.  Otherwise  compute  the  solution  x  as 

o 

=  M  ^L,  if  a  priori  is  not  to  be  added 

x  =  (M  +  P  "'■(L  +  P  ^x  )  if  a  priori  is  to  be  added, 
o  o  o  o 

10.  Then,  X  =  X  +  x 

o  o  o 

_  /*V 

11.  Replace  Xq  with  X^  and  go  to  step  0  for  the  next  iteration. 

A 

With  the  solution  X^  one  can  construct  the  trajectory  of  the 
target,  provided  the  assumed  acceleration  model  adequately  describes 
all  the  target  maneuvers  during  the  entire  observation  interval.  The 
target  maneuver  acceleration  model  assumed  for  this  investigation  is 
a  constant  acceleration,  and  hence  will  not  describe  even  simple 
maneuvers  such  as  a  steady  turn  with  constant  speed.  Consequently, 
if  the  target  executes  a  maneuver  within  the  time  span,  AT,  over 

A 

which  the  m  observations  were  taken,  then  the  solution  X  will 

o 

deviate  from  the  true  state  at  the  epoch  time  due  to  the  unmodeled 
acceleration.  Under  such  circumstances,  one  approach  to  obtain  a 
reasonable  trajectory  for  the  target  would  be  to  split  the  whole  ob¬ 
servation  set,  consisting  of  m,  observation,  into  several  (say  l) 
batches,  each  consisting  of  a  subset  of  the  original  m  measurements, 
and  then  compute  a  least-squares  solution  for  each  batch  successively 


'  V 
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by  propagating  the  solution  from  one  time  epoch  to  the  time  epoch 
of  the  next  batch.  Fig.  5.1  illustrates  this  idea.  The  arcs  can  be 
divided  based  on  equal  time  intervals  or  an  equal  number  of  observa¬ 
tions  (i.e.,  n^  =  n ^  =  ...  =  n^) .  In  either  case  the  number  of  ob¬ 
servations  in  each  arc,  n^,  should  be  selected  so  that  the  information 
T  -1 

matrix  (H  R  H)  is  full  rank  and  does  not  lead  to  numerical  diffi¬ 
culties  while  computing  the  matrix  inverse.  However,  the  time  span 
of  each  arc  should  not  be  long  compared  to  the  possible  maneuver  in¬ 
tervals.  In  the  work  reported  here,  we  have  assumed  individual  arcs 
having  a  constant  number  of  observations,  i.e.,  n^  =  NBTCH,  i=l, .,,,£. 
Algorithm  A  is  applied  to  each  arc  by  replacing  m  by  n.,  and 

X  by  X  ,  and  then,  iterating  the  individual  arc  solution  if  de- 
i 

sired.  The  solution  of  arc  i  at  the  epoch  t  is  then  propagated 

i 

to  the  epoch  time  t  of  the  next  arc  by  the  equation 

°i+i 


'i+i 


<Ht  ,  t  )X  ,  i  =  1,  2,  . . .  ,£ 
o .  .  o  o 
i+l  i  i 


(5.1) 


Thus,  the  initial  state  for  the  arc  (i+l'1  will  be  X  with 

°i+l 

a  priori  error  being  zero.  The  a  priori  emr  covariance  matrix  for 
this  initial  state  is  computed  as 


K  =  *(t  ,  t  )(h^r"1h  +  p  W(t  ,  t  ) 

o  •  i ,  o ,  ,  o.  i  i  i  o.  o  ,  o. 

i+l  i+l  l  l  i+l  i 


(5.2) 


~  T  -1  — 1  -1 

The  estimate,  covariance  pair  [X^  ,  (H^R^  +  Pq  )  ]  provide  the 

i  i 

estimate  of  the  trajectory  at  a  certain  interval  of  time  which  is 
analogous  to  the  extended  sequential  filter  solution.  Since  the  solu¬ 
tions  X  are  obtained  by  the  least-squares  method,  the  resulting 
°i 

trajectory  may  be  called  a  ’batch-sequential’  solution. 
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Batch  Estimate  with  Process  Noise 

Inclusion  of  the  effects  of  process  noise,  w(t) ,  in  the  batch 
or  batch-sequential  solution  is  the  next  step  to  be  considered.  The 
solution  to  Eq.  (2.1)  with  w  ^  0,  is  given,  formally,  as 

/k+l 

=  ^k+l’  tk^Xk  +  J  <Ktk+1>T)B(T)w(T)dT  (5.3) 

Ck 

Presence  of  the  second  term  complicates  the  computation  of  the  least- 
squares  solution.  One  simplifying  assumption  would  be  to  consider 
the  noise  w(t)  as  a  piece-wise  random  constant  function  of  time  with 
zero  mean  and  specified  covariance  for  each  time  segment.  Further 
assumptions  and  the  derivation  of  the  batch  solution  in  the  presence 
of  process  noise  are  given  in  Appendix  A.  The  computation  algorithm 
for  this  case  is  as  follows. 


Algorithm  B  —  Given  the  initial  state  X^,  the  measurement  set, 

Y.  ,  ...,  Y  ,  to  be  processed,  the  a  priori  error  x  in  the  initial 
1  m  r  r  o 

state  and  the  a  priori  covariance  Pq: 

0.  Initialize  an  n x  n  (n  being  the  dimension  of  the  state  vector) 
and  an  n x  1  matrices  M  =  0  and  L  =  0  respectively,  k  =  1 
1.  Read  the  observation:  t,  y,  a 
Let  tk  =  t  ;  YR  =  Y  ;  oR  =  a 


At 

=  t,  - 

t,  , 

k 

k-1 

2. 

Compute 

♦<V 

t  ) 

0 

3. 

Compute 

X(ck) 

-  *(tk 

3G(X,t) 

3X 

4. 

Compute 

Hk = 

J 
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5.  Compute  the  observation  Y  and  the  residual  (0  -  c) 

Ck 


6.  Stack  the  measurement  partials  matrix,  and  the  residuals 

H(k)  =  Hk$(tk,  tQ)  ;  y (k)  =  yR 

where  H(k)  and  y(k)  indicate  the  elements  of  the  k*"*1  row  of 
H  and  y,  respectively. 

7.  Compute  the  kth  row  of  the  inverse  weighting  matrix,  p  ,  where: 


8.  Increment  k:  k  =  k+1 
If  k  <_  m,  go  to  step  1 

9.  (otherwise)  Compute  the  solution  xq: 
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This  procedure  forms  the  information  arrays 


r«(D 

!  H(2) 


H  - 


H(m) 


Ty(l) 

t 

y(2) 


and  y  = 


y(m) 


and  the  associated  inverse  weighting  matrix,  T,  is  given  as 


r  = 


P11  +  R1  P12 


lm 


pml 


p  +  R 
Kmm  m 


With  H,  y,  and  T  determined,  the  solution  is  given  by  the  expression 

x  =  (HTr_1H  +  P-1)"1(HTr~1y  +  P_1x  )  , 
o  o  J  o  o 

p  =  (HTr_1H  +  p-1)"1 
o  o 

An  approximation  to  Algorithm  B,  which  saves  some  of  computation 
time  required  to  invert  the  m*m  matrix,  T,  is  obtained  by  neglecting 
all  the  non-diagonal  terms  in  the  T  matrix.  For  such  an  assumption 
r  1  is  obtained  by  m  scalar  divisions.  This  will  be  exactly  the 
same  as  Algorithm  A  except  for  Step  6.  Thus,  an  approximation  to 
algorithm  B,  denoted  as  algorithm  C  can  be  obtained  as  follows. 
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Algorithm  C  --  All  steps  are  exactly  the  same  as  in  Algorithm  A,  except 


-1 


Step  6  which  is  as  follows.  Compute  the  new  weight  for  the 


measurement : 


“k  ■  °k +  p 


kk 


where 


pkk  =  V  z  ( 

kk  k  i=1  tj 


f  1 


r‘i 


$(k,x ) B (t) dx  Q 


i-1 


i-1 


BT(i)$T(k,T)dr)}H^ 


i-1 


All  of  the  above  algorithms  can  be  used  in  either  a  batch  or  a  batch- 
sequential  solution. 

Least-Square  Solution  by  Matrix  Triangularization 

Since  the  theory  for  the  approach  was  outlined  briefly  in  Chapter 
2,  attention  in  this  section  will  be  devoted  to  the  computational 
algorithm.  Again  the  approach  can  be  adapted  to  obtain  either  a 
batch  or  a  batch-sequential  solution.  Furthermore,  this  method  can 
be  applied  to  each  of  the  three  cases  discussed  in  the  previous  sec¬ 
tion,  and  we  will  denote  the  corresponding  algorithms  as  Algorithm  A' , 
B' ,  C',  respectively. 

Algorithm  A'  and  C'  —  Given  X  ,  {Y, , . . . ,Y  },  x  ,  P  ,  perform  the 
—  -  o  1  moo 

following  computations 

0.  Set  m  =  0 
o 

•  If  there  is  no  a  priori  {x^,  >  then  go  to  Step  1.  Otherwise, 

compute  the  upper  triangular  square  root  matrix  U  of  Pq  using 

T 

the  Cholesky  decomposition  method  so  that  Pq  =  UU 

•  Invert  U  and  let  R  =  U  ^ 

•  Compute  z  =  Rx^ 

•  Move  the  upper  triangular  matrix  R  and  the  vector  z  to  the 
information  array  A. 


A  =  — 


m  =  n 
o 


1.  Set  k  =  1 


2.  Read  observation:  t,  Y,  o 


Let  t,  =  t  ;  Y.  =  Y  ;  o,  =  0 
k  k  k 


At  -  \  ~  Vi 


3.  Compute  •  tQ) 


4.  Compute  X(t.)  =  4> ( t,  ,  t  )X 
k  k  o  o 


5.  Compute  H 


3G(X,t) 


|X  =  x(tk) 


6.  Compute  the  observation  Y  and  the  residual  (0-c)  y,  =  Y,  -  Y 

Ck  k  k  Ck 

7.  Compute  the  scale  factor  to  normalize  the  observation  partials  and 
the  residuals  (0-c) : 


Algorithm  A'  —  a*  = 


Algorithm  C'  —  a*  = 


/ak  +  Pkk 


where 


•  t.  t, 

~  k  f1  T  T  ~t 

Pkk  =  \  *  Z  <  $(k,T)B(x)dT  .  Qi_1  I  B  (x)$  (k,x)dx)}Hk 

1=1  ti-l  ti-l 
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8.  Stack  the  measurement  partials  and  the  scaled  observation  devia¬ 
tions  : 

K(k)  -  »X*(tk,  to> 
y(k)  -  o^yk 

9.  Increment  k:  k  =  k+1 
If  k  <_  m,  go  to  Step  1 

10.  Compute  the  solution  x  : 

o 

(a)  Move  the  matrices  H  and  y  to  A  such  that 


A(m  +1,  1)  =  H(l)  and  A(m  +1,  n+1)  =  y(l) 


Then  depending  on  whether  a  priori  is  added  or  not,  we  have 


A  =  - 1 - 


H 

n 


m 


or  A  =  [H  j  y]  m 
n  1 


(b)  Apply  Givens  Transformation  [11]  to  A  to  obtain 


TA  = 


R  1  z 

i 

T 

0  I  e 
n  1 


,  where  T  is  orthogonal 


m+m  -n 
o 


(c)  The  solution  is  computed  as  x  =  R  and  the  covariance 

o 

rp 

of  the  solution  is,  P  =  R  R 

o 


Algorithm  B1  —  This  algorithm  proceeds  exactly  same  as  Algorithm  A' 
up  through  step  5.  Then, 
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6.  Stack  the  measurement  partials  matrix  and  the  observation  deviation 
yk  33 

H(k)  =  V(tk,  tQ) 

yOO  =  yk 

7.  Compute  (part  of)  the  inverse  weighting  matrix  p: 


P  = 


Pll+Rl  P12  *  *  *  plk 


.kl 


Pkk+Rk 


where  =  a  .  That  is,  for  i  =  1,  k,  compute 


Pik=  H  {  E  ( 

1  £=1  t 


t2,  j:£ 

*(i,T)B(T)dT  .  Q£_1  J  BT(T)<I>T(k,T)dT)}Hk 
£-1  t£-l 


8.  Increment  k:  k  =  k+1 
If  k  _<  m  go  to  Step  1 

9.  Compute  the  solution  x  : 

o 

The  arrays  H  and  y  have  to  be  normalized  using  the  mxm 
inverse  weighting  matrix,  T,  where 


r 


P11  +  R1  P12  '  ' 


Pml 


p  +  R 
mm  m 


! 


45 


(a)  To  accomplish  this  task,  compute  the  Cholesky  decomposition 

of  T  =  U  UT  so  that 

P  P 


-1  -T  -1 

r  =  u  u 

p  p 

(b)  Compute  A  =  and  z  =  U  ^y 


(c)  Move  the  matrices  A  and  z  to  (the  information  array)  A 
such  that 

A(m  +1,  1)  =  A(l,  1)  and  A(m  +1,  n+1)  =  z(l) 
o  o 


Then,  depending  on  whether  a  priori  is  added  or  not,  we  have 


R  I  z  n 

i 


A  i  z  m 


or  A  =  [A  i  z]  m 
n  1  1 


(d)  Apply  Givens  Transformation  to  A  to  obtain 


TA  =  i— 


e i  m+m  -n 
o 


n  1 


(e)  The  solution  is  given  by  x^  =  R  z  and  its  error  covar- 


iance  is  given  by  Pq  =  R  R 


Sequential  Filtering 

As  painted  out  previously,  for  the  problem  of  estimating  the 
trajectory  of  a  moving  target,  the  most  appropriate  algorithm  appears 
to  be  the  Extended  Kalman  Filter  algorithm  given  in  Eq .  (2.21), 
rather  than  the  standard  Kalman  Filter  solution  given  in  Eq.  (2.18). 


In  the  Extended  Kalman  Filter  solution,  after  processing  each 


1 


observation,  the  trajectory  will  be  rectified  using  the  estimate  of 
the  state.  In  accordance  with  the  dynamic  model  given  in  Eq.  (3.6), 
the  Dvariance  propagation  from  one  time  point  to  the  next  is  given 
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where 


Pk  $(tk’  tk-l)Pk-l<1’  (tk’  tk-l)  +  Fk 


P.  =  P (k/k-1) 
k 


P,  ,  =  P(k-l/k-l) 
k-1 


Fk  =  I  $(tk>T)Q(T)<t>  (tk,T)dt 


Assuming  Q(t)  to  be  a  constant  diagonal  matrix  and  using  the  state 
transition  matrix  given  in  Eq.  (3.8),  the  integral  in  Eq .  (5.5)  can 
be  carried  out  analytically,  to  obtain 


qf (At) 


0  q 


0 


0  q. 


20 


0  q , 


0  % 


0  q. 


0  q 


1M5 


0  (5.6) 


0  q. 


o  q. 


o  q. 


0  q 
u  ql  2 


0  qL(At) 


0  q. 


0  q 


(At)2 


0  q9  (At) 
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where  At  =  t  -  t.  ,  and  q_,  q,  ,  q„  are  defined  in  Eqs.  (3.5) 
k  k-i  t  1  Z 

and  (3.2).  The  computational  algorithm  for  the  extended  sequential 
solution  can  be  expressed  as  follows. 


Algorithm  D  —  Given  the  initial  state  X  ,  number  of  measurements  m 

—  -  0 

to  be  processed,  a  priori  estimate  of  the  error  in  initial  state  Xq, 

and  the  a  priori  error  covariance  matrix,  P  , 
r  o 

0.  Initialize  the  state  and  the  covariance  matrix: 

X  =  X  +  x 
o  o  o 

P  =  P  ;  set  k  =  1 
o  o 

1.  Read  the  observation:  t,  Y,  a 

Let  tk  =  t  ;  Yk  =  Y  ;  CJk  =  a  and  Atfc  =  tR  -  t^ 

2.  Compute  the  state  transition  matrix  3>(tk>  tk_^) 

3.  Compute  Fk 

4.  Propagate  the  state  and  the  covariance: 


\  ■  *<v  vA-i 


Pk  =  ^k*  tk-l^Pk-l<l1  ^k’  tk-l^  +  Pk 


5.  Compute  measurement  partials: 


h  =  3C(X,t) 
“k  9X 


X  = 


6.  Compute  the  observation  Y  and  the  observation  deviation  y  , 

Ck  R 


where 
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7.  Compute  the  gain  and  the  estimate: 


Kk '  Vk'wJ  +  °krl 


sk  ■  Vk 

8.  Update  the  state  and  covariance  matrix: 

\-\  +  \ 

K  ■  (I  -  W5k 

9.  Increment:  k  =  k+1 

If  k  <  m  go  to  Step  1,  otherwise  stop. 

Adaptive  Sequential  Filtering 

Though  in  general  the  Extended  Kalman  Filter  is  very  stable, 
in  some  cases  where  a  non  linear  dynamic  model  is  in  error  or  incom¬ 
plete,  the  filter  performance,  will  degrade  and  in  some  cases  the 
estimate  will  diverge  from  the  true  trajectory.  One  of  the  remedies 
for  such  a  problem  is  to  adapt  an  'adaptive  filtering'  method  which 
forces  a  lower  bound  on  the  unreliably  decreasing  state  error  covar¬ 
iance  matrix  by  introducing  fictitious  noise  in  the  dynamics,  to 
'cover'  the  model  error.  The  variances  of  such  fictitious  state  noise 
are  estimated  using  the  filter  residuals.  The  method  is  described 
by  Jazwinsky  [14].  The  dynamic  model  considered  for  the  problem 
discussed  in  this  report,  though  not  non  linear,  is  an  unstable  linear 
model  and  is  incomplete  so  far  as  the  target  maneuvers  are  concerned. 
Hence,  application  of  the  Extended  Kalman  Filter  algorithm  may  en¬ 
counter  the  above  mentioned  divergence  for  some  scenarios.  As  a 


consequence,  adaptive  filter  equations  described  in  [14]  have  been 
implemented  in  the  computer  simulation  program.  The  equations  used 
for  coding  the  algorithm  are  summarized  in  Appendix  B.  Since  at  the 
present  the  performance  of  the  filter  with  the  adaptive  state  noise 
compensation  has  not  been  completely  evaluated,  this  topic  will  not 
be  considered  further. 


6.  Numerical  Results 


Simulation  Procedure 

The  measurement  simulation  is  performed  as  follows.  A  repre¬ 
sentative  configuration  of  positions  for  a  set  of  sensors  is  assumed 
in  a  rectangular  coordinate  system.  A  trajectory  for  the  target, 
with  typical  maneuvers,  is  obtained  by  integrating  the  equations  of 
motion  with  an  assumed  acceleration  model  and  initial  conditions.  At 
selected  intervals  of  time,  the  doppler  frequency  and  the  bearing  angles 
with  respect  to  the  sensors  are  computed.  To  obtain  simulated  meas¬ 
urements,  which  are  representative  of  real  observations,  the  computed 
measurements  are  corrupted  by  additive  Gaussian  random  noise.  The 
Gaussian  noise  is  obtained  from  a  sequence  of  numbers  which  are  com¬ 
puted  as  follows.  The  random  number  generator  RANF,  which  is  a 
Fortran  Library  Function  on  the  Cyber  170/750  system  at  the  University 
of  Texas,  is  used  to  obtain  a  random  number  £  which  is  uniformly 
distributed  over  the  interval  (0,1).  According  to  the  Central  Limit 
Theorem,  the  sum  of  a  large  number  of  independent  and  identically 
distributed  random  variables  is  a  Gaussian  random  variable  [15]. 

Since  £  has  a  mean  of  1/2  and  variance  1/12,  the  random  variable 

N 

£  =  I  £  -  N/2  is  considered  Gaussian  with  zero  mean  and  unit 

i=l  1 

variance  for  N  =  12  .  Thus,  calling  the  RANF  function  twelve  times 
and  adding  the  returned  values  to  -6.0  ,  a  zero  mean  Gaussian  random 
number  with  unit  variance  is  obtained,  which  is  then  multiplied  by  the 
given  measurement  standard  deviation  (  of  for  frequency  and  cr^  for 
bearing  angle).  The  resulting  random  number  is  added  to  the  computed 
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measurement  to  obtain  the  sir.  lated  observation.  Other  methods  of 
generating  random  numbers  are  discussed  in  Reference  [16]. 

Two  basic  maneuver  histories  are  considered  in  this  investigation. 
Fig.  6.1  shows  the  first  of  the  target  maneuver  trajectory  and  the 
sensor  locations.  As  mentioned  earlier,  the  target  motion  is  considered 
in  two  dimensions  only.  The  target  is  assumed  to  have  zero  acceleration 
along  the  straightline  paths  and  travels  with  constant  speed.  Conse¬ 
quently,  during  the  turn  and  the  circular  motion  it  will  have  non-zero 
acceleration  components.  Thus,  the  trajectory  shown  in  Fig.  6.1  rep¬ 
resents  a  target  moving  with  a  constant  speed  of  5  m/sec  along  a 
straightline  path  through  the  origin  of  the  coordinate  system. 

After  maintaining  a  steady  course  for  500  seconds,  the  first  maneuver 
executed  is  a  45°  turn.  The  45°  turns  are  executed  with  constant 
values  for  acceleration  components  maintaining  the  magnitude  of  the 
velocity  to  be  5  m/sec.  T.he  circular  motion  is  executed  with  a  con¬ 
stant  turn  rate  of  0.5  deg/sec  and  speed  5  m/sec.  The  total  simulation 
time  is  1900  seconds. 

Variations  of  the  frequency  and  bearing  angle  measurements  with 
time,  computed  along  such  a  trajectory,  are  shown  in  Figures  6.2  and 
6.3,  respectively.  These  measurements  are  computed  from  the  array  of 
sensors  located  2  km  apart,  along  the  sides  of  a  right  triangle,  as 
shown  by  dots  in  Fig.  6.1  and  the  middle  sensor  of  the  array  (sensor 
No.  3)  is  at  10  km  from  the  origin.  The  coordinates  of  the  sensor 
locations  for  this  case  are  given  in  Table  6.1.  It  can  be  seen 
clearly  (at  least  in  the  frequency  plot)  that  the  different  sigments 
of  each  curve  correspond  to  the  various  maneuvers  or  straightline 
motions  of  the  target.  Note  that  for  the  first  500  seconds, there  are 
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only  three  distinct  lines  in  the  frequency  plot;  this  is  due  to  the 
symmetry  of  the  sensor  locations  with  respect  to  the  first  segment 
of  the  target  trajectory.  In  other  words,  during  this  time  period, 
the  range  rate  and  hence  the  frequency  from  sensors  1  and  5  are  eq 'al 
and  those  from  sensors  2  and  4  are  equal. 

Though  the  frequency  and  the  bearing  angle  measurements  are 
computed  with  respect  to  all  the  sensors  simultaneously,  as  shown 
in  Figures  6.2  and  6.3,  observations  corresponding  to  a  specified 
selection  of  sensors  and  to  specified  intervals  of  time  could  be  saved 
for  processing.  Such  a  capability  allows  one  to  study  the  effects 
of  measurement  interval  and  the  number  of  measurements  from  different 
sensors  on  the  accuracy  of  the  estimate.  The  simulated  observations 
consisting  of  frequency  measurements  from  all  five  sensors  and 
bearing  angle  measurements  from  only  two  sensors  (sensor  No.  1  and 
sensor  No.  5)  are  processed  as  a  nominal  case.  The  frequency 
measurements  are  sampled  every  10  seconds,  whereas  bearing  angle 
measurements  are  sampled  every  30  seconds.  This  amounts  to  a  total  of 
955  frequency  measurements  and  128  measurements  over  a  period  of  1900 
seconds.  Variations  in  sensor  locations  and  in  data  rate  are  considered 
to  study  the  effects  of  geometry  on  the  trajectory  estimation.  For 
all  cases  considered,  a  standard  deviation  of  0^  =  0.1  Hz  for  the 
frequency  measurement  and  0g  =  5  degrees  for  the  bearing  angle 
measurement  are  assumed  to  compute  Gaussian  random  numbers.  It  should 
be  remarked  that  the  noise  variance  for  the  angle  measurement  depends 
on  the  relative  range  between  the  target  and  the  sensor  and  that  for 
the  frequency  measurement  depends  on  the  relative  range  and  the 
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resolution  of  the  measuring  device.  The  quality  of  the  measurements 
degrades  with  increasing  range  [17].  The  measurement  standard 
deviations  attainable  in  practice  vary  from  3  degrees  to  6  degrees 
for  angle  measurements  and  from  0.002  Hz  to  0.2  Hz  for  frequency 

measurements.  For  the  scenario  described  above,  the  relative  range  j 

lies  between  approximately  2.4  km  and  10  km  during  the  entire  period  j 

of  simulation  and  hence  the  assumed  constant  values  of  5  degrees  for  i 

the  angle  and  0.1  Hz  for  the  frequency  measurement  standard  deviations 

can  be  considered  to  be  realistic  to  pessimistic.  The  reference 

frequency  for  the  Doppler  shift  measurement  is  assumed  to  be  300  Hz, 

and  the  speed  of  sound,  c  ,  in  the  ocean  is  assumed  to  be  1530  m/sec. 

Numerical  Results 

Fig.  6.4  compares  the  estimated  trajectory  (the  solid  line)  with 
the  true  trajectory  (the  dashed  line)  for  the  case  described  above, 
which  is  referred  to  as  Case  A.l.  The  estimated  trajectory  is  obtained 
by  the  Extended  Kalman  Filter.  The  initial  conditions  for  the  state 
propagation  are  the  same  as  those  used  for  trajectory  simulation,  and 
the  measurement  noise  standard  deviations  used  for  processing  are  also 
the  same  (oD  =  5  degrees,  o  *  0.1  Hz)  as  those  used  for  observation 
simulation.  The  initial  state,  the  diagonal  elements  of  the  a  priori 
state  error  covariance  matrix  and  the  process  noise  covariance  matrix, 
which  were  used  to  obtain  the  estimated  trajectory  in  Fig.  6.4,  are  tab- 

A 

ulated  in  Table  6.2.  The  error  in  the  estimate  (Xj_  -  X  ,  i  =  2, 

true 

.  .  .  ,  7)  for  the  components  of  position,  velocity  and  acceleration 
are  shown  in  Figures  6.5  through  6.10.  The  dashed  lines  in  all  these 
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figures,  which  are  symmetric  about  the  time-axis,  represent  the  one- 
sigma  standard  deviations  (+  a)  for  the  errors  in  the  respective  com¬ 
ponents.  The  errors  in  the  position  and  velocity  components  are  ran¬ 
dom  and  bounded  by  the  one-sigma  boundary  except  for  occasional  peaks. 

The  errors  in  the  acceleration  components  are  also  random,  but  show 
a  definite  trend  in  the  mean,  which  can  be  correlated  to  the  acceler¬ 
ations  of  the  target  during  its  various  maneuver  phases.  The  times 
of  occurrence  of  maneuvers  and  the  accelerations  during  these  man¬ 
euvers,  that  were  used  to  stimulate  the  trajectory,  are  shown  also  in 
Figures  6.9  and  6.10.  It  is  clear  that  the  simple  Brownian  motion 
model  for  the  dynamics  is  able  to  follow  the  trends  in  the  actual 
acceleration,  and  a  close  inspection  of  the  Figures  6.4  through  6.10 
indicates  that,  for  this  scenario,  the  accuracy  of  the  estimated  tra¬ 
jectory  is  quite  satisfactory. 

The  Effect  of  Sensor  Geometry 

In  order  to  see  the  effects  of  the  sensor  location  geometry,  the 
distance  between  consecutive  sensors  is  reduced  from  2  km  to  700  m,  and 
simulated  observations  for  this  case  were  obtained,  holding  all  other 
parameters  constant.  The  sensor  No.  3  is  at  a  distance  of  10  km  from 
the  origin.  Sensor  No.  3  is  kept  at  the  same  location  and  the  others 
are  moved  symmetrically  towards  it  as  indicated  by  the  stars  in  Fig.  6.1 
The  coordinates  for  these  sensor  locations  are  given  in  Table  6.3.  The 
result  obtained  by  processing  the  observation  set  obtained  with  this 
geometry  (Case  A.2)  is  shown  in  Fig.  6.11.  Comparing  Fig.  6.4  and 
Fig.  6.11,  it  is  obvious  that  the  wider  the  senses  are  spread,  the 
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better  the  tracking  results.  For  the  sensor  locations  selected  for 

Case  A. 2,  another  observation  set  (referred  to  as  Case  A. 3)  was 

generated  by  including  bearing  angle  measurements  from  all  the  5 

sensors  every  10  seconds,  instead  of  every  30  seconds  from  only  two 

sensors.  Consequently,  this  set  consists  of  955  frequency  and  128 

angle  measurements  for  Case  A.l  and  Case  A. 2.  The  result  obtained 

when  Case  A. 3  observation  set  was  used,  is  shown  in  Fig.  6.12.  A 

significant  improvement  in  the  estimated  trajectory,  compared  to  the 

one  shown  in  Fig.  6.11,  is  obtained.  The  same  sets  of  values  for 

X  ,  P  and  Q  given  in  Table  6.2  were  used  for  Case  A. 2  and 
o  o 

Case  A. 3.  Comparisons  between  the  above  three  cases  indicate  that  in 

terms  of  estimation  accuracy  and  the  cost  of  acquiring  and  processing 

observations,  it  is  better  to  spread  the  sensor  locations  as  far  as 

feasible  than  to  obtain  more  measurements  from  closely  located  sensors. 

Keeping  the  same  orientation  of  the  sensor  locations,  the  whole 

array  is  moved  away  from  the  starting  position  of  the  target,  so  that 

the  sensor  No.  3  is  15  km  away  from  the  origin.  For  this  scenario 

(which  is  the  same  as  Case  A. 1) ,  three  cases  with  the  sensors 

2  km  apart  (Case  A. 4),  sensors  700  ra  apart  (Case  A. 5)  and  sensors 

700  m  apart  but  bearings  from  all  sensors  every  10  seconds  (Case  A. 6), 

were  considered .  The  results  are  shown  in  Figures  6.13  through  6.15. 

The  values  for  X  ,  P  and  Q  in  Table  6.2  are  used  for  all  three 
o  o 

cases.  Sensor  location  coordinates  for  Case  A. 4  are  given  in  Table  6.4 
and  those  for  Case  A.  5  and  Case  A. 6  can  be  found  in  Table  6.5.  Figures 
6.13  through  6.15  confirm  the  conclusion  reached  in  the  previous 
paragraph.  Further,  it  is  to  be  noted  that  an  increase  in  range  leads 
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to  an  effective  decrease  in  the  angular  resolution  obtained  by  the 
sensor  array  and  hence  the  estimated  trajectories  shown  in  Figures 
6.13  through  6.15  are  much  worse  compared  to  the  ones  shown  in  Fig. 
6.4,  6.11  and  6.12,  respectively. 


Effects  of  Errors  in  the  Initial  Estimates 

In  each  of  the  above  cases,  a  perfect  initial  state  is  assumed  to 
compute  the  estimated  trajectory  and  the  estimates  show  the  ability  of 
the  algorithms  to  track  the  motion,  once  an  acceptable  trajectory  has 
been  obtained.  The  results  demonstrate  the  applicability  of  the  man¬ 
euver  model  and  the  sequential  estimation  algorithm  to  the  problem  of 
tracking  the  motion  of  a  maneuvering  target.  The  trajectory  and  the 
errors  in  the  estimates  of  the  position  components  shown  in  Figures 
6.16  through  6.18  are  the  result  of  processing  the  first  observation 
set  (Case  A.l),  but  with  errors  in  initial  state  and  are  to  be  com¬ 
pared  with  those  shown  in  Figures  6.4  through  6.6.  An  initial  state 
with  errors  of  -300  m  in  the  x-component  and  -1000  m  in  the  y-component 
of  the  position  and  -3.535534  m/sec  in  each  of  the  velocity  components, 
was  used  to  initiate  the  filtering  process.  The  values  of  the  initial 

state  (X  )  of  the  diagonal  elements  of  the  a  priori  state  error 
o 

covariance  matrix  (Q)  used  for  this  case  are  given  in  Table  6.6.  As 
can  be  seen  from  Figures  6.17  and  6.18,  it  takes  about  120  seconds 
or  so  for  the  transients  to  disappear,  and  from  then  on,  the  estimated 
trajectory  stays  close  to  the  true  trajectory,  as  in  the  case  of  Fig. 
6.4.  The  noncircular  loops  in  Fig.  6.16  as  opposed  to  the  circular 
loops  in  Fig.  6.4  are  due  to  the  fact  that  the  x-  and  y-axis  are 
drawn  to  different  scales  in  the  former,  whereas  the  scales  are  the 
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same  in  the  latter.  Note  the  change  in  scale  for  the  vertical  axes 
in  Figures  6.5  and  6.6.  Similar  perturbations  in  initial  state  will 
be  considered  for  the  second  type  of  target  maneuver  history  in  order 
to  demonstrate  the  stability  of  the  Extended  Kalman  Filter. 

Comparison  of  Algorithm  Performance 

In  order  to  compare  the  filter  methods  described  in  Chapter  5, 
a  second  scenario  with  a  different  trajectory  and  sensor  orientation 
is  considered.  A  simulated  observation  tape,  corresponding  to  the 
trajectory  and  sensor  locations  (indicated  by  dots)  shown  in  Fig.  6.19, 
was  obtained  from  Tracor,  Inc.  As  part  of  a  joint  effort  to  analyze 
filtering  techniques  for  target  trajectory  estimation,  this  scenario 
was  used  to  evaluate  the  filter  algorithms  described  in  Chapter  5. 
However,  in  order  to  be  able  to  vary  some  parameters  in  the  analysis, 
noisy  measurements  simulated  at  the  University  of  Texas  for  the  same 
trajectory,  as  shown  in  Fig.  6.19,  are  used  to  obtain  the  results 
shown  in  Figures  6.22  through  6.30.  Values  for  the  measurement 
noise  standard  deviations,  reference  frequency,  speed  of  sound,  and 
the  turning  rate  are  all  the  same  as  before,  but  the  speed  is  8  m/sec 
instead  of  5  m/sec.  Thus,  the  trajectory  is  computed  assuming  zero 
acceleration  along  the  straight  line  paths.  The  initial  position  of 
the  target  is  at  (+  10  km;  -  5.6  km);  the  target  moves  along  a  straight 
path  parallel  to  y-axis  at  a  constant  speed  of  8  m/sec  for  the  first 
700  seconds  and  then  makes  a  45°  turn  with  the  above  speed  and  a 
constant  turn  rate  of  0.5  deg/sec.  The  acceleration  components  will 
be  nonzero  during  this  turn.  After  completing  the  turn,  it  proceeds 
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along  the  45  line,  as  shown  in  Fig.  6.19,  at  the  same  constant 
speed.  The  sensor  location  coordinates  are  given  in  Table  6.7.  The 
total  simulation  time  is  1800  seconds.  Frequency  measurement 

a 

sampled  at  every  10  seconds  from  all  the  sensors  and  bearing  angle 
measurements  sampled  at  every  30  seconds  from  sensor  No.  1  and  sensor 
No.  5  are  saved  for  processing  and  consequently  the  observation  set 
will  have  a  total  of  905  frequency  measurements  and  122  bearing  angle 
measurements.  The  observation  set  simulated  for  the  scenario  described 
above  will  be  referred  to  as  Case  B.l. 

The  frequency  and  bearing  angle  measurements  simulated  in  Case  B.l 
have  been  plotted  (without  the  noise  being  added)  against  time  and  are 
shown  in  Figures  6.20  and  6.21,  respectively.  The  frequency,  being  a 
function  of  the  range  rate  of  the  target  (see  Eq.  4.1),  varies  as  the 
range  rate  changes.  The  range  rates  corresponding  to  all  the  sensors 
are  negative  to  start  with,  and  increase  steadily  as  the  target  moves, 
and  hence  the  computed  values  of  the  frequencies  decrease  continuously. 

The  sharp  corners  are  due  to  the  jump  in  the  values  for  the  accelera¬ 
tions  components  occurring  at  the  start  (700  sec.)  and  at  the  end 
(790  sec.)  of  the  target  maneuver,  and  the  steep  linear  increase  in 
the  frequency  values  occurs  during  the  turn.  The  line  joining  the 
sensors  3,  4,  and  5  is  perpendicular  to  the  trajectory  at  a  point 
towards  the  end,  at  which  the  range  rates  of  the  target  with  respect 
to  these  sensors  become  zero.  Hence,  the  frequency  curves  correspon¬ 
ding  to  these  sensors  concur  at  a  point  with  the  ordinate  300  cycles/sec., 
as  can  be  observed  in  Fig.  6.20.  The  bearing  angles  (measured  from 
positive  x-axis  towards  positive  y-axis)  with  respect  to  all  the  sensors 
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increase  continuously,  as  the  target  moves  along  the  trajectory,  and 
are  shown  in  Fig.  6.21.  Taking  the  bearing  angle  as  modulo  2tt  causes 
the  discontinuities  in  the  curve,  which  occur  when  the  target  crosses 
from  one  quadrant  to  the  next  for  the  respective  sensors.  Finally,  note 
that  in  this  figure  also,  the  curves  corresponding  to  the  sensors 
3,  4,  and  5  intersect  at  one  point  due  to  the  fact  that  the  position 
of  the  target  at  this  time  lies  on  the  same  line  joining  these  three 
sensors . 

Trajectory  solutions  obtained  by  processing  the  above  observation 

set  Case  B.l.  using  the  Extended  Kalman  Filter  and  the  "batch-sequential" 

filter  are  shown  in  Figures  6.22  through  6.24  in  order  to  compare  the 

two  filter  algorithms.  The  initial  state  ,  the  a  priori  state 

error  covariance  matrix  P  ,  and  the  process  noise  covariance  matrix 

o 

Q  used  to  obtain  the  estimated  trajectories  shown  in  the  above  three 
figures,  are  tabulated  in  Table  6.8.  The  initial  state  is  the  same  as 
that  used  for  simulating  the  true  trajectory  (shown  by  dashed  lines), 
and  the  measurement  noise  standard  deviations  used  for  processing  the 
measurements  are  also  the  same  as  those  used  for  simulation  of  the 
observations.  One  common  feature  among  all  the  three  figures  is  that 
the  estimated  trajectories  stay  closer  to  the  true  trajectory  in  the 
earlier  portion  than  during  the  rest  of  the  estimation  period.  This 
can  be  cxplaii  ed  by  noting  that  the  initial  state  for  the  estimation 
process  is  exact,  yielding  a  perfect  trajectory  to  start  with,  and 
also  the  filter  gains  are  large  in  the  beginning,  which  decrease  when 
the  state  error  covariance  matrix  decreases  with  time.  Another  common 
feature  among  the  three  figures  is  the  large  deviation  of  the  estimated 


trajectories  from  the  true  trajectory,  occurring  approximately  around 
a  point  (10  km  -  2.5  km).  At  this  point,  the  frequency  measurement  be¬ 
comes  insensit ive  to  the  target  position  and  velocity  when  the  radius 
vector  joininj  the  target  and  a  sensor  is  normal  to  the  velocity 
vector  of  the  target.  Hence  the  frequency  measurements  from  sensor 
No.  1  will  have  very  little  or  no  information  about  the  target  state 
when  the  y-coordinate  of  the  target  is  near  -1.4  km.  This  could  be  the 
reason  for  the  above  mentioned  large  deviation  in  the  estimated 
trajectories.  Fig.  6.22  shows  a  comparison  between  the  true  and  the 
estimated  trajectory  obtained  by  using  the  Extended  Kalman  Filter, 
whereas  the  true  and  the  estimated  trajectories  obtained  by  using  "batch- 
sequential"  algorithm  are  compared  in  Figures  6.23  and  6.24.  The  only 
difference  between  the  latter  two  trajectories  is  that  the  one  in 
Fig.  6.23  does  not  have  process  noise  in  the  dynamics,  whereas  the  one 
in  Fig.  6.24  includes  the  effects  of  non-zero  covariance  for  the  process 
noise  in  the  acceleration  components.  For  both  trajectories,  the 
observation  set  (Case  B.l)  is  divided  into  34  small  batches,  each  con¬ 
sisting  of  30  observations  and  batch  solutions  are  obtained  for  each 
sub-arc,  as  described  in  Algorithm  B  of  Chapter  5.  The  solution  of 
each  sub-arc  Ls  iterated  4  times  within  the  sub-arc  and  the  estimate 
converges  in  2  or  3  iterations  for  all  arcs.  Besides  being  not  so 
accurate  compared  to  the  EKF  estimates,  these  trajectories  take  an 
appreciable  amount  of  computing  time.  For  example,  the  execution  times 
for  the  trajectories  shown  in  Figures  6.22  through  6.24  are  10.8  sec., 
59.6  sec.,  21  p.7  sec.,  respectively.  Comparing  Fig.  6.23  with  Fig.  6.24, 
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it  is  obvious  that  having  process  noise  in  the  dynamics  shows  a 
definite  improvement  in  the  solution.  From  the  above  discussion  of 
Figures  6.22  through  6.24,  one  can  conclude  that  the  performance  of 
the  EKF  algorithm  is  comparable  to  the  "batch-sequential"  algorithm 
both  in  terms  of  accuracy  and  computing  time. 

In  order  to  check  the  performance  of  the  EKF,  the  initial  con¬ 
ditions  to  the  filter  were  perturbed  and  solutions  were  obtained. 

A  few  sets  of  initial  errors  were  considered  and  the  results  of  pro¬ 
cessing  the  observation  set  Case  B.l,  for  one  typical  set  of  errors 

are  shown  in  Figures  6.25  through  6.27.  The  initial  state  X  ,  the 

o 

a  priori  state  error  covariance  matrix  Pq  ,  and  the  process  noise 
covariance  matrix  Q  used  to  obtain  these  figures,  are  given  in 
Table  6.9.  During  the  initial  few  seconds  the  error  in  the  x-component 
of  position  oscillates  between  -3.3  km  and  2.4  km  and  that  in  the 
y-component  between  -1.0  km  and  1.9  km.  These  figures  show  that  after 
about  90  seconds  of  initial  transients,  the  estimated  trajectory 
converges  to  the  true  trajectory,  indicating  that  a  reasonable  error 
in  initial  state  will  not  cause  a  total  failure  of  the  EKF  algorithm. 
However,  the  performance  of  the  filter  is  degraded  in  that  the 
deviation  of  the  estimated  trajectory  from  the  true  trajectory  shown 
in  Fig.  6.25  is  larger  compared  to  that  in  Fig.  6.22.  The  increase 
trend  in  the  later  part  of  the  one-sigma  curve  shown  in  Fig.  6.27 
may  not  signify  the  divergence  of  the  filter,  since  that  in  Fig.  6.26 
decreases  steadily  and  the  errors  in  both  the  components  of  position 
are  well-bounded  within  the  one-sigma  curves. 
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In  order  to  verify  the  statement  made  earlier  regarding  sensor 

geometry  for  this  scenario  also,  the  sensor  locations  are  spread  over 

a  wider  area  (as  marked  by  the  stars  in  Fig.  6.19  and  the  coordinates 

given  in  Table  6.10)  and  a  set  of  simulated  observations  (Case  B.2) 

were  obtained.  Using  the  set  of  values  for  X  ,  P  and  Q  given 

o  o 

in  Table  6.9,  i.e. ,  with  the  same  set  of  initial  errors  as  for  Figures 
6.25  through  6.27,  the  results  obtained  by  processing  this  observa¬ 
tion  set  are  shown  in  Figures  6.28  through  6.30.  When  compared  with 
Fig.  6.25,  a  significant  improvement  can  be  noticed  in  the  estimated 
trajectory  shown  in  Fig.  6.28.  Though  in  this  case  also  it  takes 
about  90  seconds  for  the  initial  transients  to  disappear,  the  errors 
in  Figures  6.29  and  6.30  are  bounded  with  the  one-sigma  curves  and 
are  smaller  compared  to  those  shown  in  Figures  6.26  and  6.27,  re¬ 
spectively.  (Note  the  difference  in  scale  between  the  corresponding 
figures.)  Hence,  it  can  be  concluded  again  that  a  wider  station 
location  geometry  reduces  estimation  errors. 


Table  6.1  Sensor  Location  Coordinates  for  Case  A.l 


sensor 

No. 


x-coordinate  (m) 


7071.07 

7071.07 

7071.07 

5071.07 

3071.07 


coordinate  (m) 


3071.07 

5071.07 

7071.07 

7071.07 

7071.07 


Table  6.2  Values  of  X  ,  P  and  Q  for  Processing  Case  A.l 

o  o 

through  Case  A. 6  (Figures  6.4  through  6.15) 


Component 
of  State 

Initial 

State  X 

o 

Diagonal 
of  P 

o 

Diagonal 
of  Q 

X1 

300.0 

1.0 

0.0 

X2 

0.0 

1.0E+05 

0.0 

X3 

0.0 

1.0E+05 

0.0 

X4 

3.535534 

1.0E+01 

0.0 

X5 

3.535534 

1 . 0E+01 

0. 0 

X6 

0.0 

1.0E-02 

1.0E-05 

X7 

0.0 

1.0E-02 

1.0E-05 

Table  6.3  Sensor  Location  Coordinates  for  Cases  A. 2  and  A. 3 


Sensor  No. 


x-Coordinate  (m) 

y-Coordinate  (m) 

7071.07 

5671.07 

7071.07 

6371.07 

7071.07  i 

7071.07 

6371.07 

7071.07 

5671.07 

7071.07 

Table  6. A  Sensor  Location  Coordinates  for  Case  A. 4 


Sensor  No. 


x-Coordinate  (m) 

y-  Coordinate  (m) 

10606.6  1 

6606.6 

10606.6 

8606.6 

10606 . 6 

10606.6 

8606.6 

10606.6 

6606.6 

10606.6 

Table  65  Sensor  Location  Coordinates  for  Cases  A. 5  and  A. 6 


Sensor  No. 


x-Coordinate  (m) 

y-Coordinate  (m) 

10606.6 

9206.6 

10606.6 

9906.6 

10606,6 

10606.6 

9906.6 

10606.6 

9206.6 

10606.6 
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Table  6.6  Perturbed  Values  of  XQ  and  Values  of  P  and  Q  for  Case 
A. 1  (figures  6.16  through  6.18)  0 


Components 
of  state 

Initial 
state  X 

0 

Diagonal 
of  P 

0 

Diagonal 
of  Q 

X1 

300.0 

1.0 

0.0 

X2 

-300.0 

1 . 0E+08 

0.0 

X3 

-1000.0 

1.0E+08 

0.0 

x4 

0.0 

1 . 0E+02 

0.0 

X5 

0.0 

1.0E+02 

0.0 

X6 

0.0 

1.0E-02 

1.0E-05 

_ i 

0.0 

1.0E-02 

1.0E-05 

Table  6.7  Sensor  Location  Coordinates  for  Case  B.l 


Sensor  No. 

x-Coordinate  (m) 

y-Coordinate  (m) 

1 

0.0 

-1400.0 

2 

-700.0 

-700.0 

3 

-1400.0 

0.0 

4 

-700.0 

700.0 

5 

0.0 

1400.0 

•  ■ .  •<  .4  .v* , 


Table  6.8  Values  of  X 


and  Q  used  for  Case  B.l 


Components 
of  state 


Initial 
state  X 

o 


300.0 

10000.0 

-5600.0 

8.0 

0.0 

0.0 

0.0 


Diagonal 
of  P 

o 


1.0 

1.0E+05 

1.0E+05 

1.0E+01 

1.0E+01 

1.0E-02 

1.0E-02 


Diagonal 
of  Q 


0.0 

0.0 

0.0 

0.0 

0.0 

1.0E-05 

1.0E-05 


Table  6.9  Perturbed  Values  of  X  and  Values  of  P  and  Q 

for  case  B.l  and  B.2  °(Figures  6.25  through  6.30) 


Components 
of  state 


Initial 
state  X 

o 

Diagonal 
of  P 

o 

Diagonal 
of  Q 

300.0 

1.0 

0.0 

10000.0 

1.0E+08 

0.0 

-6600.0 

1.0E+08 

0.0 

0.0 

1.0E+02 

0.0 

0.0 

1.0E+02 

0.0 

0.0 

1.0E-02 

1.0E-05 

0.0 

1.0E-02 

1.0E-05 

X  uV,  *1G 

.aa  2a.  aa  4Q.qq  ea.  oa  bq.  qo.  lqq.  aa  12Q.  qq 
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SIMULATED  SCENARIO  A ;  CASE  A 
TRAJECTORY  FOR  OBSERVATION  GENERATION 

r  2'  3' 

•  •  *  #  * 

I  2  3 

*4' 

*5' 

4* 


Fig.  6.1 
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DVX.  SIG  rvx)  (M/SEC) 

.00  -0.50  0.00  0.  50  1.00  1-50  2.00 


74 


SIMULATED  SCENARIO  A;  CASE  A  I 
EXTSEQ  ONLY;  EXACT  XO;  P0=E5;  Q  =  E-5 
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S  I  MULRT  !:D  SCENARIO  A;  CASEAI 
EXTSEQ  ONLY;  EXACT  XO;  P0=E5;  Q  =  E  -5 
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SIMULATED  SCENARIO  A;  CASE  A- 1 
EXTSEQ  ONLY;  EXACT  XO ;  P0  =  E5;  Q  =  E-5 
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SIMULATED  SCENARIO  A?  CASE  A  2 
EXTSEQ  ONLY;  EXACT  XO;  PD-E5 ;  Q-E-5 


/ 

/ 


-50.  OQ  SO^OQ  15 Q.  00  250.  OQ  350.  00  450.  OQ  550 

Y  CM]  Hicr 


Fig.  6.11 


•**  <w 


* 


5 I MULRTED  5CENRRI G  R;  CASE  A  3 
EXT5EQ  ONLY ;  EXRCT  XCU  PO-E5  ;  Q=E-5 


so.  oa 


r  - —i -  ~  v 

i  iso.  ca  250.  oa  350.0a 
TCM)  *  I  Ql 


450.  00  550.  00 


Fig.  6.12 


X  (Mi  #1G1 

-50.30  50.00  150.00  250.00  350.00  450.00  550.00 


81 


g  5  I MULRTED  SCENARIO  R ? CASE  A  5 
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SIMULATED  SCENARIO  A ;  CASE  A6 
EXTSEQ  ONLY  r  EXACT  XO;  P0-E5?  Q=E-5 
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SIM  SCENRB I  0  F) ;  CASE  A  I  ;  P0  =  E8,  Q  =  E-5 
EXTSEQ  ONLY  ;  0X0= -300,  DYO= -1 000,  DVO= -5 
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SIMULATED  SCENARIO;  CASE  B 
TRAJECTORY  FOR  OBSERVATION  GENERATION 
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S I MULRTED  SCENARIO;  CASE  Bl 
FREQUENCY  (NO  NOISE)  FROM  5  SENSORS 
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SIMULATED  SCENARIO;  CASE  Bl 
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SIMULATED  SCENARIO;  CASE  B  I  0PT=1 
SEQ8CH  134.  30)  ;  EXACT  XO;  P0  =  E5 


Fig.  6.23 
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SiMULRTEO  SCENRRIQ  ;  CASE  B  l  P-E8;  Q-E-5 
EXTSEQ  ONLY  ;  DY0--1000  CM)  ;  DYDTQ--8  CM/S) 
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5 1  MULRTif  D  SCENRRI  0  ;  CASE  B  I  ;  P=E8;  Q---E-5 
EXTSEQ  ONLY;  DYQ--IOOO  CM)  ;  DYDT 0- -8  CM/S ) 
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SIMULATED  SCENARIO?  CASE  B2 ?  P-E8?  Q-E-5 
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7.  Conclusions  and  Recommendations 

The  problem  of  estimating  the  trajectory  of  a  slow  moving  maneuv¬ 
erable  target  using  passive  measurements  from  an  array  of  stationary 
sensors,  has  been  investigated  in  the  previous  chapters.  Based  on  the 
results  discussed  in  Chapter  6,  the  following  conclusions  can  be 
drawn  regarding  the  mathematical  model  and  the  algorithms  used  in  the 
study.  These  are  listed  below  along  with  recommendation  of  topics  to 
be  studied  in  the  future. 

Conclusions 

•  The  choice  of  a  rectangular  coordinate  system  is  advantageous 
in  that  the  target  dynamics  is  linear  for  the  acceleration 
model  considered  here. 

•  Modelling  the  acceleration  components  in  rectangular  coordinates 
as  Brownian  motion  process  is  adequate  to  estimate  the  traject¬ 
ory  of  the  target  executing  typical  maneuvers.  Though  it  is  a 
simple  model,  it  is  able  to  closely  follow  the  actual  maneuver 
acceleration  curves  when  used  with  the  EKF  algorithm. 

•  The  Extended  Kalman  Filter  performs  well  for  the  scenarios  con¬ 
sidered  and  provides  satisfactory  results.  It  is  especially 
well  suited  to  tracking  the  targets  motion  once  the  estimation 
initiation  transient  has  dissipated. 

•  The  'batch-sequential'  filter  method  with  the  process  noise 
modification  estimates  a  trajectory  which  is  in  general  agree¬ 
ment  with  the  estimate  obtained  using  EKF.  However,  the  compu¬ 
tation  time  for  estimating  the  trajectory  using  this  algorithm 
is  larger  than  the  EKF  execution  time  by  a  factor  of  about  20. 
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When  the  process  noise  is  not  included  in  the  'batch-sequential' 
filter  solution,  the  solution  is  not  as  accurate  as  the  EKF 
solution  during  and  following  the  maneuvers. 

•  Though  the  measurements  were  simulated  with  pessimistic  values 
for  the  measurement  noise  standard  deviations,  the  trajectory 
estimated  by  EKF  converges  to  the  true  trajectory  even  when 
the  initial  state  is  perturbed.  The  errors  in  the  initial  state 
cause  initial  transients  in  the  estimated  trajectory,  which  dis¬ 
appear  within  a  short  time  (about  5%  of  the  total  time  period). 

4  The  effect  of  sensor  geometry  is  an  important  factor  in  the 
accuracy  of  the  estimates  of  the  target  motion.  Measurf ments 
made  from  sensors  located  as  far  apart  as  feasible,  yield  better 
estimates  than  those  obtained  from  closely  located  sensors. 

The  geometric  orientation  of  its  sensors  is  far  more  important 
than  the  number  of  observations. 

Recommendations  for  Further  Study 

Further  investigations  must  be  made  to  understand  the  character¬ 
istics  of  the  measurements.  The  dependence  of  the  measurement  standard 
deviations  on  the  relative  range  is  an  important  aspect  that  should  be 
modeled  in  future  simulation  studies.  The  model  formulation  should 
be  extended  to  three  dimensions.  The  characteristic  of  the  signal 
propagation  in  the  medium,  for  example,  the  multi-path  effects  on  the 
signal  received  at  the  sensors,  should  be  studied.  Initial  results 
obtained  with  adaptive  filtering  wherein  the  process  noise  covariance 
is  also  estimated,  appear  promising  and  should  be  given  a  more  thorough 
evaluation.  Different  models  for  the  dynamics  should  be  investigated; 


for  example,  modelling  the  acceleration  components  as  an  adaptive 
first  order  Gauss-Markov  process.  This  model  will  increase  the  dimen¬ 
sion  of  the  state  vector,  but  allow  a  better  estimate  of  the  acceler¬ 
ation. 
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Random  Constants.  (Note  that  w(t)  as  shown  here  is 
a  scalar  and  represents  w^(t)  or  w^(t)  or  w^(t) 
referenced  in  Eq.  3.7) 
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Now,  at  a  measurement  time  point  t^,  the  measurement  is  related 

(after  linearization)  to  the  state  at  t^  by  the  relation 


=  Vk +  ek 


(A. 5a) 


where  is  the  measurement  noise,  with  variance  R^.  Substituting 

Eq.  (A. 4)  in  Eq.  (A. 5a), 


~  k  f1 

Hk$(k,0)xQ  +  Z  J  <J>(k,T 


)B(T)dt  +  ek 


+  n. 


o  k 


(A. 5b) 


where  the  combined  noise  term 

~t. 


\  =  Hk  E 
K  i=l 


<Kk,x)B(T)dx 


f'i-i 


wi-l  +  ek 


(A. 6) 


and  =  iyKk.O). 

In  order  to  simplify  the  notation,  define 


*k" 


,-to 


n  i  j  2 

$(k,x)B(x)dx  |  [  <&(k,i)B(x)di  j. 

I 


•I 

>C1 


„T  r  T  T 
U,  =  [w  w  . 
k  o  1 


T  i 

•  Vi1 


1  fk 


<J>(k,x)B(x)dT 


k-1 


(A. 7a) 
(A. 7b) 


so  that  we  can  write 


\  •  Wk +  \ 


(».8) 


After  processing,  say,  m  scalar  measurements,  the  equations 
as  in  (A. 5b)  can  be  placed  in  the  following  matrix  form  as 


y  ■  Hx  +  H 
1  o 


(A. 9) 
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where 


y  = 


— 1 
v; 

H* 

_ 1 

1 

sc 

i-* 

_ 1 

1 

iH 

C* 

j _ 

CM 

»2 

n2 

• 

,  H  = 

• 

and  n  = 

. 

• 

• 

. 

• 

• 

• 

ym 

H 

m 

nm 

_ 

_  _ 

each  H^,  i  =  1,  2,  m  being  a  lxn  row  vector.  To  compute 

the  least-squares  estimate  of  the  epoch  state  xq  from  Eq.  (A. 9), 

given  an  a  priori  epoch  state  x  and  its  covariance  P  ,  from  the 

o  o 


data  equation, 


x  =  x  +  e  where  E{e  e  }  =  P 
o  o  o  o  o  o 


(A.  10) 


the  following  procedure  is  used.  Augument  the  measurements  in 
Eq.  (A. 9)  with  the  a  priori  information  contained  in  Eq.  (A. 10)  to 
obtain: 


r  ™ 

-  - 

X 

i 

1  e 

o 

X  + 

° 

y 

H 

—  _ 

o 

f 

-U 

1 _ 

z  =  X  x  +  v 
o 


(A. 11) 


The  covariance  matrix  for  the  augmented  error,  v  ,  can  be  obtained  as 


f  =  E{vvT} 


E  (e  e  ^)  E(e  nT) 
oo  o 


e(tie^)  E(nnT) 


(A. 12) 
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Note  that, 


E{e  nT}  =  e{g  (nTn~  •••  nT)} 
o  ol/  m 


E{e  (e^ 
o  1 


m  T  fr  fi»  m  (p  fr 

■  Sn>>  +E(e„<Wi  U2*282 


llVfi1) 

mm  m 


Since  e  ,  the  error  in  the  a  priori  state  is  uncorrelated  with  the 
o 

measurement  noise  (e^,  i  =  1*  •••»  m)  and  with  the  process  noise 
(w^,  i  =  0,  1,  ....  m-1) ,  it  follows  that 


E{e  nT)  =  E{nET}  =  0 

o  o 


(A. 13a) 


The  diagonal  terms  of  f  are 


E{e  e  }  =  P 

o  o  o 


(A. 13b) 


and  E(nn  }  =  F  ,  where 


Ed^nJ)  ...  E(n1n^) 


E(n  nn)  ...  E(n jr) 
ml  mm 


(A. 13c) 


t  H  T 

Now,  the  (i,  j)  element  of  E(nn  )  is: 


E^nf)  =  H^EdJjUj)^  +  Hi*1E(UieJ)  +  ECe^K^H*  +  E^eJ) 
•  +  Ri5i) 


where,  assuming  that  the  process  noise  w^,  i  *  1,  ...  H  and  the  ob- 

T  T 

servation  noise  are  uncorrelated,  it  follows  that  E[e^Uj]  =  E[U^£j]  = 
for  all  i  and  j.  Substituting  for  ip  and  U,  from  Eqs.  (A. 7a)  and 
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(A.  7b)  ,  p  is  defined  as 

pij  =  ViE<uiu>K 


=  H.E  (  £  $(i.T)B(T)dT.  w„  '[  i  w  T,  (  BT(T)«>T(j,T)d-ri \] 

^F^-l  ”/ 1 10=1  m_  m-1  '■» 

min(i,j)  r*^  f£  t  t  '] -T 

=  H.  T,  <I>(iJT)B(T)dT.Q1?  (j,l)dt|)H 

^4  4 


min(i,j) ( 


4>(i,T)B(T)dT.Q 


(A. 14) 


where  =  E{w^w^}  .  Finally  then  the  elements  of  T  are  given  as 


T..  =  E{n.n4}  =  p . .  +  R.6..  and 
1J  1  j  ij  1  1J 


P11  +  R1  P12  Plm 


p  +  R 
mm  m 


(A. 13c) 


From  Eqs.  (A. 12)  and  (A. 13),  i ^  follows  that 


P  0 
o 


o  r 


(A.  15) 


The  least-squares  solution  corresponding  to  Eq.  (A. 9)  and  (A. 10)  is 
given  then  by  the  following  expression. 


a  , ,T~-1,  .  -1, Tf-1 

x  =  (a  r  a)  a  r  z 
o 


=  (HTr_1H  +  p“1)_1(HTr"1Y  +  P_Xx 


+  P  X  ) 
o  o 


(A. 16) 
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It  is  to  be  noted  that  the  mx  m  matrix,  T,  is  not  a  diagonal  matrix 
and  therefore  the  computation  of  x^  by  Eq.  (A. 16)  involves  inversion 
of  first,  an  m*  m  matrix  and  then,  an  n*n  matrix  where  m  is 
the  number  of  observations  processed  and  n  is  the  dimension  of  the 
state  vector. 


Appendix  B 

Process  Noise  Variance  Computation 
for  Adaptive  Filtering 


The  main  idea  involved  in  arriving  at  the  equations  for  esti¬ 
mating  the  process  noise  covariance  (Q)  is  based  on  the  assumption 
that  the  filter  residuals  be  consistent  with  their  statistics.  In 
other  words,  defining  the  (predicted)  residuals  as 

'k+t  ■  - El: W?k)  ■  l>0  <»•» 


where  y^+£  is  the  measurement  deviation  at  time  t^+£  defined 
in  Eq.  (2.2)  and  E{yk+^/Y  }  is  the  expected  value  of  the  measure¬ 
ment  deviation  at  time  tk+^  based  on  measurements  up  to  time  t^, 
it  is  required  that 


2 

rk+S. 


E{r  k+jl},  *  =  1,  2, 


(B.2) 


j  and  hence  r^  ^  are  assumed  to  be  scalars  in  this  discussion. 
From  Eq.  (B.2)  one  derives  the  process  noise  covariance  required 

/s 

at  time  t^  to  propagate  the  state  error  covariance  matrix 

to  the  next  time  point  as  given  in  Eq.  (3.10).  Note  that  such 

an  adaptive  filter  will  have  a  time  lag  (tm  ) ,  since  (k+m  )  observa- 

tions  have  to  be  obtained  before  prediction  to  time  t^+^  can  be  made 

The  state-noise  compensation  matrix  Q  for  the  dynamic  model  defined 

in  Eq.  (3.6)  is  a  7*  7  matrix  with  non-zero  values  for  the  first, 
t  h  th 

6  and  the  7  diagonal  elements  and  with  zero  for  all  other  elements 

Here  a  3x  3  diagonal  matrix  Q'  is  considered  with  non-zero  diagonal 

elements  and  consequently  an  appropriate  matrix  B  is  introduced  by 

T 

pre-multiplying  Q'  so  that  Q  =  BQ'B  and 
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Using  the  same  notations  as  in  Chapter  2,  for  the  filter  residual,  we 
have 

E{rk+£}  =  \+£*(k+£.  k)\  $T(k+£’k)^+£  +  \+£ 


£ 

T.  <J>(k+£,k+i)BQ'BV(k+£,k+i) 

i=l 


~T 

\+£  + 


k+£ 


£  =  1 ,2  , .  . .  ,m 


q 

(B.3) 


Imposing  the  condition 


r2  =  E{r2  } 

k+£  1  k+r 


£  =  1,2 


’“q 


will  give  rise  to  the  following  set  of  equations: 


rk+£  "  \+£$(k+£,k)\'Ii  (k+£,k)\+£"  ak+£  =  \+£ 

£  T  T 
E  <J>(k+£,k+i)GQ'G  4>  (k+£,k+i) 

i=l 


^  £  =  l,2,...,mq  (B.4) 


Each  equation  corresponding  to  £  =  l,2,...,m^  is  a  scalar  equation. 
However,  the  scalar  on  the  right  hand  side  of  (B.3)  can  be  written 
as  the  product  of  a  row  vector  and  a  column  vector,  so  that  the  above 
equations  can  be  put  in  the  following  matrix  form: 

€  =  Afdiag  Q' ]  (B.5) 

where  £  is  *  1,  A  is  m^ x  3  and  [diag  Q ’ ]  is  3x1.  Then 
the  solution  to  Eq. (B.4)  can  be  computed  as 
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(diag  Q’j  =  (ATA)  AT(£ 


(B.6) 


where 

mator 


(P)  is  the  pseudo-inverse  of  P. 


This  leads  to  the  esti- 


diag  Q' 


[c,  6  <  0 

|jiiag  Q'  , 


-+  each  element  of  6  0 

otherwise 


(B.7) 


subiect  to  the  condition  that  if  (q..)  <  0,  then  set  (q..)  =  0,  since 

33  33 

the  diagonals  are  variances  and  cannot  be  negative. 
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